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ABSTRACT 


The  performance  of  a  Kalman  filter  used  to  track  a  hurricane  was  substantially  im¬ 
proved  by  implementing  a  fixed  interval  smoothing  algorithm.  This  tracking  routine  was 
designed  and  implemented  in  a  microcomputer  program.  Several  tracking  scenarios  were 
simulated  and  analyzed.  Actual  storm  tracks  obtained  from  the  Joint  Typhoon  Warning 
Center  in  Guam,  Mariana  Islands,  were  used  for  this  research.  The  application  of  the 
Kalman  tracker  to  a  tropical  storm' s  wind  speed  tracking  was  also  investigated  by  using 
the  best  track  data  and  observed  data. 
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THESIS  DISCLAIMER 

The  reader  is  cautioned  that  computer  programs  developed  in  this  research  may  not 
have  been  exercised  for  all  cases  of  interest.  While  every  effort  has  been  made,  tv, (Inn 
the  time  available,  to  ensure  that  the  programs  are  free  of  computational  and  logtc  er¬ 
rors,  they  cannot  be  considered  validated.  Any  application  of  these  programs  without 
additional  verification  is  at  the  risk  of  the  user. 
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I.  INTRODUCTION 


"  Conceived  over  warm  tropical  oceans,  born  amid  torrential  thundershowers,  ami 
nurtured  by  water  vapor  drawn  inward  from  far  away,  the  mature  tropical  cyclone  is  an 
offspring  of  the  atmosphere  with  both  negative  and  positive  consequences  for  life.  Se¬ 
vere  cyclones  are  among  the  most  destructive  of  all  natural  disasters,  capable  of  annihi¬ 
lating  coastal  towns  and  killing  hundreds  of  thousands  of  people.  On  the  positive 
though  less  dramatic  side,  they  provide  essential  rainfall  over  much  of  lands  they  cross. 
It  is  difficult  to  convey  to  those  who  have  never  experienced  a  tropical  cyclone  the  hor¬ 
ror  that  great  hurricanes  can  bring  to  ships  at  sea  or  people  living  near  the  coast. 
Tropical  cyclones  cause  a  variety  of  damage  and  the  same  tropical  cyclone  often  affects 
several  nations  during  its  lifetime.  They  are  called  "Hurricanes"  in  the  Atlantic  and 
eastern  Pacific"  [Ref.  1],  Hurricanes  were  identified  by  female  or  male  names  like  Pat 
and  Tess.  These  storms  will  be  discussed  in  this  thesis.  Tropical  cyclones  are  also 
numbered  sequentially  according  to  their  starting  date.  This  numbering  system  is  used 
with  caution  when  referencing  storms  from  other  data  bases. 

This  thesis  attempted  to  improve  the  estimation  of  the  hurricane's  future  course, 
speed,  and  position  by  using  a  Kalman  filter  with  smoothing.  This  problem  is  similar 
to  the  ship  tracking  problem  which  is  discussed  in  a  previous  thesis  (Ref.  2].  'The  major 
difference  between  ship  tracking  and  storm  tracking  problem  is  the  measurement  process 
which  is  given  actual  position  coordinates  (latitude  and  longitude)  in  the  storm  tracking 
problem.  Therefore,  the  linearization  required  in  the  ship  tracking  problem  is  unneces¬ 
sary  in  the  storm  tracking  problem.  The  measurement  noise  varies  with  the  type  of  the 
sensor  (aircraft,  satellite,  and  radar). 

An  accurate  and  reliable  method  of  tracking  and  targeting  is  necessary.  The  current 
methods  used  to  track  a  storm  include  the  use  of  radar,  aircraft,  and  satellite.  However, 
the  data  may  or  may  not  be  available  when  needed  for  a  number  of  reasons.  As  an  ex¬ 
ample,  aircraft  may  not  be  available  due  to  flight  restrictions.  A  Kalman  filter  with  a 
fixed  interval  smoothing  algorithm  can  be  used  to  track  a  storm.  The  smoothing  algo¬ 
rithm  is  an  off-line  calculation  that  uses  all  measurements  taken  during  a  time  interval 
0S.k  <.M  to  improve  the  estimate.  By  having  a  more  accurate  assessment  of  what  the 
storm  has  done  in  the  past,  we  will  be  better  able  to  predict  ahead  and  estimate  a  storm's 
future  course,  speed,  and  position. 
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The  estimation  of  the  wind  speed  is  as  important  as  the  storm  position  estimate.  In 
an  clfort  to  estimate  the  possible  damage  n  hurricane's  sustained  winds  and  storm  surge 
could  do  to  a  coastal  area,  the  Kalman  filter  and  the  smoother  was  used  to  estimate  the 
t  wind  speed  and  to  categorize  the  hurricane.  If  the  wind  speed  estimate  is  accurate,  a 
hurricane  is  categorized  correctly.  This  thesis  attempts  to  estimate  the  hurricane's  future 
wind  speed.  This  will  help  to  design  a  timely  warning  system. 
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II.  PROBLEM  STATEMENT 


'A.  GENERAL 

The  storm-tracking  scenario  parallels  the  ship  tracking  problem  in  that  both  prob¬ 
lems  developed  a  position,  course,  and  speed  solution  for  a  target  with  similar  system 
dynamics.  The  tracking  scenario  used  here  involves  two  storms.  The  positions  of  the 
storms  arc  given  in  x  (longitude),  and  y  (latitude)  coordinates.  This  problem  will  be 
analyzed  using  state  space  methods.  Given  the  longitude  and  latitude  (the  measure¬ 
ments)  received  by  a  radar,  aircraft,  or  satellite,  we  are  interested  in  estimating  the  lo¬ 
cation,  course,  and  speed  of  the  storm  (the  states  of  the  plant).  The  state  variables  for 
this  plant  arc  x ,  Sc,  y,  and  y. 

B.  SYSTEM  MODEL 

This  system  can  be  described  by  the  state  space  equation 


&k+ 1  “  'i'k&k  +  wk 


(2.1) 


where 

state  vector  to  be  estimated, 

4>k~  state  transition  matrix  which  describes  how  the  states  of  the  dynamic  system  arc 
related,  and 

w\  —  random  forcing  function  with  a  covariance  matrix  Qk  that  is  defined  as 
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The  state  vector  is 


(2.2) 


(2.3) 


and  the  system  state  equations  are 


3 


k  +  twkl 


(2.4) 


**“ 

'l 

T 

0 

o’ 

‘x~ 

X 

0 

1 

0 

0 

X 

y 

k+ 1  “ 

0 

0 

1 

T 

y 

ym 

0 

0 

0 

I 

y_ 

C.  MEASUREMENT  MODEL 

The  measurements  are  linearly  related  to  the  state  variables,  using  the  measurement 
equation 


(2.5) 


Since  the  x  and  y  position  states  are  observed  directly  and  given  by  latitude  and  longi¬ 
tude  position  coordinates,  the  measurement  equation  can  be  written  as 
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(2.6) 


where  the  measurement  noise  v*  has  a  variance  associated  with  the  source  of  the  meas¬ 
urement.  In  this  thesis,  mean  deviation  (nm)  of  satellite-derived  tropical  cyclone  posi¬ 
tions  from  best  track  positions  (PCN  values)  were  used  in  the  calculation  of  the 
measurement  noise  covariance  matrix  for  the  satellite  data.  The  measurement  noise 
covariance  matrix  values  and  PCN  values  are  shown  in  Table  1.  The  equation  used  in 
this  calculation  is 


Rk  —  ( Mean  deviation )2  (2.7) 


Table  1.  THE  MEASUREMENT  NOISE  COVARIANCE  MATRIX  VALUES 
FOR  SATELLITE 


PCN 

Mean  Deviation 

1.  or  2 

16 

256 

3,  or  4 

30 

900 

5.  or  6 

40 

1600 

The  measurement  noise  covariance  matrix  values  were  calculated  by  using  the  ac¬ 
curacy  number  for  the  aircraft  and  radar  data.  Equation  (2.8)  was  used  for  aircraft  data 
and  Equation  (2.9)  was  used  for  the  radar  data 

* 
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(2.8) 


Rk  =  (( Navigational )2  +  ( Meteorological )J) 

Rk  —  ( Radar  Accuracy  )2  (2.9) 

where  the  radar  accuracy  numbers  are  shown  in  Tabic  2 


Table  2.  TITS  MEASUREMENT  NOISE  COVARIANCE  MATRIX  VALUES 
FOR  RADAR 


Accuracy  Number 

Radar  Accuracy 

R> 

i,4,  orG 

10 

100 

2,  5,  or  F 

15 

225 

3,  6,  or  P 

25 

625 

7,  or  Blank 

30 

900 

D.  KALMAN  FILTER 

Basically,  the  Kalman  filter  takes  an  a  priori  estimate  of  the  states,  projects  it  ahead 
in  time  to  some  predicted  estimate,  and  then  calculates  a  gain  vector  based  on  the  error 
covariance  of  these  estimates.  The  error  between  the  observed  measurements  and  the 
predicted  measurements  of  the  corresponding  state  estimates  is  multiplied  by  the  gain 
vector  and  the  result  is  added  to  the  predicted  state  estimates  to  give  the  best  estimate 
of  the  true  states  based  on  optimal  combinations  of  a  priori  estimates  and  current 
measurements. 

The  Kalman  filter  is  the  proper  algorithm  to  be  used  when  both  the  system  model 
and  the  measurement  model  arc  linear  functions  of  the  state  variables.  The  basic  oper¬ 
ation  of  the  filter  is  a  relatively  straightforward  recursive  process.  The  equations  used 
in  the  Kalman  filter  (Ref.  3]  are 
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&k+ 1  ~  "h  k^k 

(2.10) 

Ik  =  "k&k  + 

(2.11) 
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.I'wrV-WWm-v  (2- 1«) 

where 

*  projected  ahead  state  estimate, 

P I (*!*-»  «  projected  ahead  state  error  covariance  matrix, 

Gk  *  Kalman  gain  matrix, 

Rt  *  state  measurement  noise  covariance  matrix,  and 
//*  *  linearized  measurement  matrix. 

The  Kalman  gain  matrix  serves  to  minimize  the  mean  square  estimation  error  and 
is  an  indication  of  how  much  weight  will  be  placed  on  the  current  observation.  A  large 
gain,  indicating  a  large  error  covarince,  will  place  more  weight  on  the  current  observa¬ 
tion  as  the  filter  tries  to  correct  the  states.  The  gain  matrix  is  proportional  to  the  vari¬ 
ance  of  the  uncertainty  in  the  estimate  and  inversely  proportional  to  the  variance  of  the 
measurement  noise.  It  can  be  expressed  as 

G„  -  V-.AV  <X17> 

An  initial  velocity  estimate  is  taken  to  be  zero  since  there  is  no  velocity  information 
at  the  beginning.  The  initial  state  estimates  carry  with  them  some  error  and  it  is  this 
error,  or  rather  an  estimate  of  this  error,  that  is  used  to  construct  the  initial  error 
covariance  matrix.  The  initial  position  error  was  estimated  to  be  10  nautical  miles  in  the 
x  y  direction  and  the  initial  velocity  was  estimated  to  be  0.158  nautical  miles  per  minute. 
The  error  was  assumed  to  be  zero  mean  and  uncorrelated.  With  these  approximations, 
the  initial  error  covariance  matrix  is  given  by 

*100  0  0  0 

0  0.025  0  0 

P(0H)“  0  0  100  0 

_  0  0  0  0.025 

E.  SMOOTHING  ALGORITHM 

Smoothing  is  a  procedure  that  uses  all  of  the  state  estimates  produced  by  an  esti¬ 
mator  and  attempts  to  improve  the  accuracy  of  these  estimates  by  using  the  negative 
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time  dynamics  to  produce  the  smoothed  estimate.  The  estimator  used  here  is  the 
Kalman  filter.  The  basic  idea  behind  smoothing  is  that,  for  a  time  interval  from  0  to  K 
(K  >  A),  an  estimate  at  time  A  based  on  all  previous  estimates  up  to  time  K,  ( .v(A.w  ), 
t  will  be  more  accurate  than  an  estimate  based  only  on  the  estimates  up  to  time  A,  {xm). 
“  It  is  a  non-real  time  operation  where  the  available  data  arc  processed  to  obtain  an  es¬ 
timate  xm  for  some  past  value  of  k  "  [Ref.  4). 

Smothing  algorithms  were  categorized  into  three  groups  by  Meditch  [Ref.  5]; 

Fixed  Point  Smoothing  smooths  the  estimate  at  a  fixed  point  A  as  K  increases. 

Fixed  Lag  Smoothing  smooths  the  estimate  x(K  —  N  |  K)  at  a  fixed  delay  N  as  K  in¬ 
creases. 

Fixed  Interval  Smoothing  smooths  the  estimate  xWKi  over  the  time  interval  from  0  to 
K  where  K  is  fixed  and  A  varies  from  0  to  K. 

A  fixed-interval  smoothing  algorithm  was  used  in  this  thesis.  This  smoothing  rou¬ 
tine  provides  the  optimal  state  estimate  at  each  time  A  over  a  fixed  interval  from  0  to 
K.  The  equations  used  in  the  smoothing  algorithm  [Ref.  5]  are 


'h  ~  Pwkfi)TP{k+]\k) 

(2.19) 

*tyi|,Y)  =  £{k\k)  +  ^ki^k+\\S)  ~£(k  +  1  1  *)) 

(2.20) 

T 

l\k\M  ~  ^W)  +  dk{P \k+\\S)  -  l\k+\\k))^k 

(2.21) 

where 

Ak  *  smoothing  filter  gain  matrix, 

*=  smoothed  state  estimate  a  time  k  based  on  N  observations,  and 
Ppi v>  «  smoothed  state  error  covariance  matrix. 

At  the  begim  ing  of  the  smoothing,  the  last  filtered  estimate  becomes  the  initial 
smoothed  estimate.  The  index  A  is  decremented  by  one  for  each  pass  during  the 
smoothing  algorithm  with  the  starting  value  of  A  equal  to  the  number  of  data  points  to 
be  smoothed,  minus  one  (N  -  1  ).  Consequently,  the  tracking  program  makes  (A'  -  1) 
passes  through  the  smoothing  algorithm. 


III.  STORM  TRACKING 


'A.  GENERAL 

The  Kalman  filter  program  STORM. FOR  was  used  in  computer  simulations.  This 

program  was  originally  written  for  a  ship  tracking  problem  and  was  modified  to  use  on 

storm  tracking  problem.  The  graphing  routines  of  the  MATLA1)  were  used  to  generate 

the  graphs.  A  complete  listing  of  the  program  is  included  in  Appendix  A.  Typhoon  Tess 

and  Typhoon  Pat  were  used  for  simulations.  The  storm  tracks  used  were  obtained  from 

data  collected  at  the  Joint  Typhoon  Warning  Center  located  in  Guam.  Each  storm  is 

given  a  separate  deck  name.  Tropical  cyclones  are  numbered  sequentially  according  to 

their  starting  date  by  the  JTWC.  There  are  four  types  of  data: 

Best  T'ock  -This  file  is  the  6-hourly  storm  positions  based  on  a  post  storm, 
subjecti  *y  smoothed  path. 

Forecasts  -This  data  contains  the  real  time  storm  positions,  objective  forecasts,  and  the 
official  forecast.  Each  date-time  group  may  contain  one,  two,  or  all  three  types  of 
data. 

Forecast  Errors  -Eight  different  errors  were  computed  for  each  of  the  objective  and 
official  forecasts. 

Fixes  -Tropical  cyclone  fixes  (observations)  from  four  different  platforms  are  con¬ 
tained  in  the  data  base. 

The  position  coordinates  were  obtained  using  aircraft,  satellite,  and  radar.  The  data 
obtained  included:  raw  data  (observations);  best  track  data;  and  12,  24,  and  48  hour 
predictions.  The  raw  data  was  processed  just  as  if  it  was  real-time  observation  of  the 
hurricane.  The  first  storm,  Pat,  originated  east  of  Taiwan  in  the  western  Pacific  on  24 
August  1985.  The  warning  period  for  this  storm  was  six  days.  The  storm  traveled  1337 
nm.  The  maximum  speed  of  the  wind  was  over  107  kt  and  the  minimum  sea  level  pres¬ 
sure  was  1002  mb.  The  Typhoon  Pat  caused  significant  damage  in  southwestern  and 
northeastern  Japan;  primarly  on  the  islands  of  Kyushu  and  Hokkaido.  Kyushu  was  hit 
the  hardest  with  wind  gusts  of  107  kt.  A  total  of  23  people  were  reported  killed  with  over 
180  people  injured.  An  estimated  3000  homes  were  damaged.  Pat  also  severely  dis¬ 
rupted  transportation  by  land,  sea.  and  air. 

The  second  storm  track  analyzed  was  that  of  Typhoon  Tess  which  originated 
southeast  of  Guam  on  30  August  1985.  The  warning  period  for  this  storm  was  seven 
days.  The  storm  traveled  1470  inn  with  maximum  wind  speeds  of  over  90  kt.  The  storm 
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brought  needed  rain  to  the  Philippines  during  a  spell  of  drier  than  normal  weather.  The 
storm  also  brought  death  and  destruction.  Considerable  flooding  and  crop  damage  oc¬ 
curred  over  southern  China  as  Tess  moved  inland  [Ref.  6].  The  observed  track  of 
t  Typhoon  Pat  and  Typhoon  Tess  are  shown  in  Figure  1  and  Figure  2,  respectively. 
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B.  COMPUTER  SIMULATIONS 

1.  Typhoon  Pat 

The  best  track  of  Typhoon  Pat  is  shown  in  Figure  3.  The  best  track  positions 
,  arc  in  6-hourly  increments.  The  first  tracking  data  point  corresponds  to  the  day-time 
group  08270000Z.  Figure  4  shows  the  Kalman  filter  position  estimates  and  Figure  5 
shows  the  smoothed  position  estimates.  Figure  6  was  constructed  using  the  filtered  and 
smoothed  position  estimates.  In  general,  the  smoother  does  improve  the  track  accuracy. 
In  the  area  of  the  track  where  the  true  positions  do  vary,  the  smoother  tracking  error  is 
zero.  Specifically,  this  area  occurs  between  23°  N,  124°  E,  and  38°  N,  133°  E.  This  area 
can  be  seen  easily  in  Figure  7.  This  figure  was  constructed  by  using  the  tracking  error 
of  the  filter  and  smoother.  The  average  tracking  errors  for  this  storm  are  ±  4  nautical 
miles  for  the  filter  and  ±  2  nautical  miles  for  the  smoother  estimates. 

2.  Typhoon  Tess 

The  performance  of  the  smoother  on  the  track  of  Typhoon  Tess  was  similar  to 
that  of  Typhoon  Pat.  Figure  8  shows  Typhoon  Tess  best  track.  Typhoon  Tess  best 
track  data  are  also  in  6-hourly  increments.  The  filter  and  smoother  tracking  results  are 
shown  in  Figure  9  and  Figure  10,  respectively.  Figure  1 1  shows  the  track  results  ob¬ 
tained  with  the  Kalman  filter  and  smoothing  algorithm.  The  smoother  shows  some  im¬ 
provement  near  17.5°  N,  120°E  and  15.2°  N,  130°E.  The  filter  average  tracking  error 
increased  slightly,  to  about  ±  5  nm,  but  the  smoother  average  tracking  error  jump  to 
about  ±  5  nm.  This  is  because  the  smoother  gives  30  nautical  miles  tracking  error  near 
18.8°  N,  116°E  due  to  large  change  on  the  direction  of  Typhoon  Tess.  Figure  12  shows 
the  tracking  errors  of  the  filter  and  smoother.  It  is  observed  that  the  smoother  was 
much  less  sensitive  to  the  large  course  changes  than  the  Kalman  filter.  It  is,  therefore, 
reasonable  to  assume  that  similar  results  could  be  expected  from  the  smoother  for  a 
large  course  change  more  than  90° .  However,  the  smoother's  estimates  arc  quite  good 
over  the  entire  trajectory  and  the  estimates  closely  follow4  the  course  changes  as  in 
Typhoon  Pat. 
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Figure  3.  The  Best  Track  of  Typhoon  Pat 
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STORM  1  TRACKS  —  FIL(o)  vs  SM(x) 
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Figure  7.  Tracking  Errors  of '  the  Filter  and  Smoother  for  typhoon  Pat 
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Figure  8.  The  Best  Track  of  Typhoon  Toss 
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ST0RM2  TRACKS  —  TRU(o)  vs  FILT(x) 


Figure  9.  Filtered  Track  of  Typhoon  Toss 
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Figure  10.  Smoothed  Track  of  Typhoon  Tess 
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LONGITUDE  E — (DEGREE) 


IV.  STORM  WIND  TRACKING 


*  A.  GENERAL 

"  In  an  effort  to  estimate  the  possible  damage  a  hurricane's  sustained  winds  and 
storm  surge  could  do  to  a  coastal  area,  the  Safiir-Simpson  damage-potential  scale  was 
developed.  The  scale  numbers  are  based  on  actual  conditions  at  some  time  during  the 
life  of  the  storm "  (Ref.  7].  Table  3  shows  these  categories. 


Table  3.  SAFFIR-SIMPSON  HURRICANE  DAMAGE-POTENTIAL  SCALE 


Scale  Num¬ 
ber 

Wind 

spced(knots) 

Damage 

1 

64-82 

Damage  mainly  to  trees,  shrubbery',  and  unanchorcd 
mobile  homes. 

2 

83-95 

Some  trees  blown  down;  major  damage  to  exposed 
mobile  homes;  some  damage  to  roofs  of  buildings. 

3 

96-113 

Foliage  removed  from  trees;  large  trees  blown  down; 
mobile  homes  destroyed;  some  structural  damage  to 
small  buildings. 

4 

114-135 

All  signs  blow’n  down;  extensive  damage  to  roofs,  win¬ 
dow's,  and  doors;  complete  destruction  of  mobile 
homes;  Hooding  inland  as  far. 

5 

>135 

Severe  damage  to  windows  and  doors;  extensive  dam¬ 
age  to  roofs  of  homes  and  industrial  buildings;  small 
buildings  overturned  and  blown  away;  major  damage 
to  lower  doors  of  all  structures  less  than  4.5  m  above 
sea  level  within  500  m  of  shore. 

The  storm  wind  tracking  scenario  parallels  the  storm  tracking  problem.  The  track¬ 
ing  scenario  used  here  involves  two  storms.  This  problem  Will  be  analyzed  using  state 
space  methods.  Given  the  tropical  cyclone  intensity  values  the  observed  speed  of  the 
storm  wind  will  be  estimated  by  using  the  Kalman  filter  and  smoother.  Table  4  shows 
the  relationship  between  intensity  and  wind  speed.  The  wind  speed  was  used  directly  as 
a  measurement  for  the  best  track  data  of  the  storm.  The  state  variables  for  this  plant 
arc  w,  and  u\ 

The  system  can  be  described  by  the  state  space  equation 

iv*+,  =  4>kivk+fk  (4.1) 


23 


where 


j&*  state  vector  to  be  estimated, 

<£*=*  state  transition  matrix  wliich  describes  how  the  states  of  the  dynamic  system  are 
related,  and 

fk~  random  forcing  function  with  a  covariance  matrix  Qk  that  is  defined  as 

£[(4)!]  (4.2) 

The  state  vector  is 

*-[»]  >4-3> 

and  the  system  state  equations  are 

[s]*.-[j  r  ][»]*+ 1 

The  measurements  are  linearly  related  to  the  state  variables.  Using  the  measurement 
equation 


lk  =  Umk  +  *k 


(4.5) 


The  measurement  equation  can  be  written  as 

CIO  ][*]»  +  *  !  (4.6) 

where  the  measurement  noise  v*  has  a  variance  associated  with  tiie  source  of  the  meas¬ 
urement.  The  measurement  noise  covariance  matrix  values  arc  calculated  in  the  same 
manner  as  in  storm  position  tracking  problem  by  using  Equations  (2,8)  and  (2.9)  for  the 
aircraft  and  radar  data. 

The  initial  error  covariance  matrix  used  in  the  wind  speed  tracking  is 
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Aoi-1) 


1000000  0  0  0 

0  0.25  0  0 

0  0  1000000  0 

0  0  0  0.25 


(4.7) 


Table  4.  MAXIMUM  SUSTAINED  WIND  SPEED  AS  A  FUNCTION  OF 
FORECAST  INTENSITY  NUMBER 


Intensity 

Wind  specd(nm'h) 
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B.  COMPUTER  SIMULATIONS  ' 

I.  The  Best  Track  Data 
a.  Typhoon  Pat 

Using  the  best  track  data  wind  speed  values  as  the  measurements,  future 
wind  speed  values  were  estimated  by  the  filter  and  the  smoother.  There  is  an  initial  track 
error  due  to  the  error  in  the  initial  state  estimates.  When  the  wind  speed  increases  at 
24  hours,  the  tracking  error  decreases  and  becomes  zero  for  the  fifth  data  as  the  filter 
gains  the  wind  track.  However,  it  increases  after  90  hours  when  the  wind  speed  de¬ 
creases  very  fast  and  it  returns  to  zero  two  data  points  later  as  the  filter  regains  the  wind 
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track.  Figure  14  shows  the  filter  tracking  accuracy.  Tne  smoother  is  not  as  accurate  as 
in  the  position  estimate  due  to  the  large  change  in  wind  speed,  but  these  errors  remain 
in  the  acceptable  ranges.  The  smoother  track  is  shown  in  Figure  15.  The  average 
,  tracking  errors  arc  ±  0.5  mph  for  the  filter  and  ±  1.1  mph  for  the  smoother.  Best  track 
data  represents  the  weather  service's  estimate  of  truth  [Ref.  6].  Figure  16  compares  the 
forward  time  estimate  (filter,  FlL(o))  with  the  forward  and  negative  time  estimate 
(smoother,  SM(x))  for  Typhoon  Pat.  Figure  17  denotes  the  error  in  these  estimates. 
b.  Typhoon  Tess 

The  tracking  results  for  this  storm  are  shown  in  Figures  18-22.  From  Figure 
19  and  20,  we  can  see  how  the  Kalman  filter  and  the  fixed  interval  smoothing  improve 
the  overall  track  estimate.  During  the  overall  track  estimate,  two  large  filter  tracking 
errors  arc  detected.  This  is  shown  in  Figure  19.  In  both  instances  the  smoother  also 
gives  large  tracking  errors.  Figure  20  shows  the  smoother  estimates.  At  other  times, 
however,  the  filtered  and  smoothed  estimate  are  accurate.  Figure  21  is  the  comparison 
of  the  filter  and  smoother  estimates.  The  filter  average  tracking  error  is  ±  1.5  mph  and 
the  smoother  average  tracking  error  is  ±  2.0  mph.  Figure  22  shows  the  tracking  errors 
of  the  filter  and  smoother  estimates. 


f 


26 


Figure  13.  The  Best  Track  Wind  Speed  of  Typhoon  Pat  [Ref.  6  ] 
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STORM1  TRACKS  -  TRU(o)  vs  FILT(x) 
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TIME -(HOURS) 


STORM1  TRACKS 


Figure  15.  Smoothed  Track  of  Typhoon  Pat's  Best  Track  Wind  Speed 
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STORM1  TRACKS  -  HL(o)  vs  SM(x) 
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Figure  16.  Filtered  and  Smoothed  Track  of  Typhoon  Fat's  Best  Track  Wind  Speed 
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Figure  18.  The  Best  Track  Wind  Speed  of  Typhoon  Tess  [Ref.  0  ] 


Figure  19.  Filtered  Track  of  Typhoon  Toss'  Best  Track  Wind  Speed 


33 


TIME-(HOURS) 


Figure  20.  Smoothed  Track  of  Typhoon  Tess'  Best  Track  Wind  Speed 
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HME-(HOURS) 
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2.  The  Observed  Wind  Speed  Data 

There  was  uncertainty  in  the  observed  data  obtained  from  the  JTWC  [Ref.  6]. 
This  data  has  more  than  one  data  at  the  same  time  instant  lor  the  different  positions 
t  from  the  eye  of  the  hurricane.  This  is  shown  in  Figures  23  and  24.  There  was  a  strong 
potential  for  the  filter  to  go  unstable.  This  was  data  smoothed  using  the  Equations  (4.8) 
and  (4.9).  The  data  obtained  before  and  after  curve  fitting  is  shown  in  Figures  25  and 
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where 

zk-  measurements  to  be  smoothed,  and 
xk-  smoothed  measurements. 

a.  Typhoon  Pat 

Using  the  interpolated  data  as  an  observed  data,  tracking  results  obtained 
for  typhoon  Pat  arc  shown  in  Figures  27  and  28.  The  filter  and  smoother  estimates  the 
wind  speed  accurately.  There  is  no  potential  for  the  filter  and  smoother  to  go  unstable. 
The  accuracy  of  the  filter  is  about  70%,  and  the  smoother  is  about  65%.  Due  to  the 
instant  change  in  the  wind  speed,  the  smoother  cannot  adapted  to  this  change  easily. 

b.  Typhoon  Tess 

The  performance  of  the  filter  and  the  smoother  arc  better  in  Typhoon  Tess. 
They  estimate  the  wind  speed  very  accurately.  Again,  there  is  no  potential  for  the  filter 
and  smoother  to  go  unstable.  During  the  tracking  scenario  the  filter  gives  the  actual 
observed  value  and  the  smoother  docs  improve  the  accuracy  of  these  estimates.  The 
tracking  error  is  usually  zero  or  very  close  to  zero.  The  accuracy  of  the  filter  and 
smoother  are  almost  the  same  in  this  hurricane  which  is  about  85%.  The  tracking  re¬ 
sults  are  shown  in  Figures  29  and  30. 
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STORM1  TRACKS  -  TRU(o)  w  OBS(x) 
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figure  23.  The  Observed  Wind  Speed  at  Some  Distance  of  Typhoon  Fat 
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Figure  24.  The  Observed  Wind  Speed  at  Some  Distance  of  Tjphoon  Tess  [Ref.  6] 
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Figure  25.  The  Observed  and  Interpolated  Track  of  Typhoon  Pat 


40 


TIME(  HOURS) 


Figure  27,  Filtered  Track  of  Typhoon  Pat's  Observed  Wind  Speed 
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Figure  28.  Smoothed  Track  of  Typhoon  Pat's  Observed  Wind  Speed 
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Figure  29.  Filtered  Track  of  Typhoon  Tess'  Observed  Wind  Speed 
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V.  CONCLUSIONS 


The  purpose  of  this  research  was  to  improve  the  accuracy  and  storm  tracking  ca¬ 
pability  of  a  Kalman  filter  tracking  by  implementing  a  fixed  interval  smoothing  algo¬ 
rithm.  Two  different  tropical  storms  were  simulated  and  the  accuracy  of  the  observed, 
the  filtered,  and  the  smoothed  storm  tracks  were  analyzed  and  discussed. 

'I  he  fixed  interval  smoothing  algorithm  improved  the  position  accuracy  of  the  storm 
in  all  of  the  tracking  scenarios  simulated.  However,  the  smoothed  result  was  not  always 
the  most  accurate  for  every  storm  track.  The  smoother  did  improve  the  track  accuracy 
on  the  basis  of  the  best  track  storm  positions.  The  effectiveness  of  the  smoother  in¬ 
creased  as  the  storm  lifetime  increased  and  the  storm  course  change  decreased. 

The  storm  wind  speed  tracking  scheme  implemented  worked  well.  Ho\ve\er,  because 
this  tracking  involves  the  addition  of  a  time-varying  value  of  the  state  excitation  matrix. 
Qk ,  there  was  a  strong  potential  for  the  filter  to  go  unstable.  This  was  observed  during 
the  storm  wind  speed  tracking.  It  was  difficult  to  decide  the  value  of  Ok  and  Rk  for  ob¬ 
served  wind  speed  tracking,  because  intensity  could  not  be  observed  many  times.  This 
problem  was  solved  by  using  a  curve  fitting  method  and  then  this  data  was  used  for  in¬ 
puts  to  the  tracking  problem.  The  results  show  that  this  method  can  be  used  to  in¬ 
terpolate  the  uncertain  data  and  to  avoid  an  unstable  filter. 

The  application  of  the  Kalman  filter  tracker  to  the  storm  tracking  problem  would 
be  very  useful  in  attempting  to  predict  the  storm's  track  when  little  data  is  available,  as 
seen  in  observed  wind  speed  tracking  problem.  Then,  by  using  the  filter  and  smoothing 
algotithm,  track  of  the  stoim's  past  history  can  be  calculated  allowing  for  a  more  accu¬ 
rate  prediction  of  the  storm's  future  track.  1  here  was  no  standard  deviation  for  observed 
wind  data.  If  JTWC  can  obtain  standard  deviations  for  observed  wind  data,  this  can 
be  used.  The  wind  data  obtained  has  much  missing  data,  some  times  causing  an  unsta¬ 
ble  filter. 


APPENDIX  A.  STORM.FOR 


This  is  a  listing  of  the  STORM.FOR  program  used  to  generate  the  data  for  the 
target  tarcks  presented  in  this  thesis.  In  order  to  run  this  program,  the  ST0RM1.DAT 
or  STORM2.DAT  file  must  be  available. 


C  ***  STORM 1*** 


*********  70  RUN  ************** 


1)  ENSURE  STORM  DATA  IS  AVAILABLE 

2)  RUN  STORM 1  OR  STORM 2 

3)  COPY  OB SDATA , F I LDATA ,  &  SMDATA  -->  MATLAB  SUB-DIR. 

4)  BEGIN  MATLAB  -->  RUN  ST0RM2.  M 


************ 


****************** 


********************* 


THIS  PROGRAM  EMPLOYS  AN  ADAPTIVE  EXTENDED  KALMAN 

FILTER  WITH  A  FIXED  INTERVAL  SMOOTHING  ALGORITHM  TO  TRACK  A 

TROPICAL  STORM  USING  OBSERVED  LATITUDES  AND  LONGITUDES. 


************* 


***** 


************ 


******************************** 


C  ***VARIABLE  DEFINITIONS*** 


C  AK 

C  AKT 

C  ERG 

C  BRKKM1 

C 

C  DBRG 

C  DT 

C 

C  DTOR 

C  El ,E2 

C  E1M1.E2M1 

C 

C  E1M2.E2M2 

C 

C  FAC1 

C  G 

C  GATE1 

C 
C 

C  H 

C  HDG 

C  HI 

C  I 

C  I  MAT 


SMOOTHING  FILTER  GAIN  MATRIX 
TRANSPOSE  OF  AK 

MEASURED  TARGET  BEARING  IN  RADIANS 
PREDICTED  TARGET  BEARING  MEASUREMENT 
IN  RADIANS  BRG(K|K-1) 

MEASURED  TARGET  BEARING  IN  DEGREES 
TIME  DELAY  BETWEEN  OBSERVATIONS,  T(K) 

-  T(K1) 

DEGREE  TO  RADIAN  CONVERSION  FACTOR 
MEASUREMENT  RESIDUAL,  Z(K)  -  h’(X(K|K-l)) 
MEASUREMENT  RESIDUAL  AT  PREVIOUS 
OBSERVATION 

MEASUREMENT  RESIDUAL  TWO  OBSERVATIONS 
PREVIOUS 

RECIPROCAL  OF  VARE 

KALMAN  GAIN  VECTOR 

1. 5*STANDARD  DEVIATION  OF  RESIDUAL 

PROCESS,  USED  AS  A  GATE  IN 

MANEUVER  DETECTION 

MEASUREMENi  MATRIX 

ESTIMATED  TARGET  HEADING  IN  DEGREES 

TRANSPOSE  OF  H 

COUNTER 

4  X  «r  IDENTITY  MATRIX 
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nonooooonoooooonoooooooooooooooooooooooooooooooooci 


J 

K 

LPKK 

LPKKM1 
LXKK 
LXKKM1 
Ml, M2 

PHI 

PHIT 

PI 

PKK 

PKKS 

PKKM1 

PKKM1S 

IPKKM1S 

PSS 

R 

RANGE 

RTOD 

SPD 

TEMP 

VARE 

XDIFF 

XKK 

XKKS 

XKKM1 

XKKM1S 

XPOS 

XS 

XSS 

XT 

YD  IFF 

YPOS 

YS 

YT 

ZX 

ZY 


COUNTER 

ITERATION  INTERVAL 

STATE  COVARIANCE  MATRIX  AFTER  PREVIOUS 
OBSERVATIONS 

A  PRIORI  STATE  COVARIANCE  ESTIMATE 

STATE  ESTIMATE  AFTER  PREVIOUS  OBSERVATIONS 

A  PRIORI  STATE  ESTIMATE 

AVERAGE  OF  RESIDUALS  OVER  LAST  THREE 

OBSERVATIONS 

DISCRETE-TIME  STATE  TRANSITION  MATRIX 
TRANSPOSE  OF  PHI 
3. 141592654 

ESTIMATION  ERROR  COVARIANCE  MATRIX,  P(K|K) 
SMOOTHED  ERROR  COVARIANCE  MATRIX 
PREDICTED  ESTIMATION  ERROR  COVARIANCE 
MATRIX,  P(K | K- 1) 

PREDICTED  ERROR  COVARIANCE  MATRIX  FOR 
SMOOTHING,  P(K+1|K) 

INVERSE  OF  PKKM1S 

ERROR  COVARIANCE  MATRIX  FOR 

SMOOTHING,  P(K|K) 

MEASUREMENT  NOISE  COVARIANCE 

DISTANCE  FROM  SENSOR  TO  A  PRIORI  TARGET 

POSITION 

RADIAN  TO  DEGREE  CONVERSION  FACTOR 
ESTIMATED  TARGET  SPEED  IN  KNOTS 
TEMPORARY  STORAGE  MATRICES  USED  IN 
MATRIX  OPERATIONS 
VARIANCE  OF  RESIDUALS  PROCESS 
DISTANCE  IN  X  DIRECTION  FROM  SENSOR  TO 
A  PRIORI  TARGET  POSITION 
ESTIMATED  TARGET  STATE  VECTOR,  X(K|K) 
SMOOTHED  TARGET  STATE  VECTOR 
PREDICTED  TARGET  STATE  VECTOR, 

X(K|K-1) 

PREDICTED  TARGET  STATE  VECTOR  FOR 
SMOTHING,  X(K+1|K) 

ESTIMATED  TARGET  POSITION  IN  X 
DIRECTION 

SENSOR  POSITION  IN  X  DIRECTION 
TARGET  STATE  VECTOR  FOR  SMOOTHING, 

X( K | K) 

TRUE  TARGET  POSITION  IN  X  DIRECTION 
DISTANCE  IN  Y  DIRECTION  FROM  SENSOR  TO 
A  PRIORI  TARGET  POSITION- 
ESTIMATED  TARGET  POSITION  IN  Y  DIRECTION 
SENSOR  POSITION  IN  Y  DIRECTION 
TRUE  TARGET  POSITION  IN  Y  DIRECTION 
OBSERVED  POSITION  IN  X  DIRECTION 
OBSERVED  POSITION  IN  Y  DIRECTION 


C  VARIABLE  DECLARATIONS 
CHARACTER*!  A,B 


RE AL*4  XKK ( 4 , 1 ) , XKKM 1(4,1), LPKKM 1(4,4), LXKKM 1(4,1) 

R£AL*4  H(2 ,4) ,HT(4,2) ,G(4,2) ,TEMP1( 2, 1) ,TEMP2( 2,4) ,TEMP3( 2,1) 


4S 


REAL*4  TEMP4(4,2) ,TEMP5(4,1) ,TEMP6(4,4) ,TEMP7(4,4) 

REAL*4  PKK(4,4) ,PKKM1(4,4) ,Z(2,1) 

RE AL*4  LXKK( 4,1), LPKK( 4,4) ,XS( 10) ,YS( 10) ,DBRG( 10) ,BRG 
REAL*4  PHI (4, 4) ,PHIT(4,4) ,IMAT(4,4) ,XT,YT 
REAL*4  GATE 1 ,E( 2, 1) , VARE( 2,2), IVARE( 2,2) 

RE AL*4  DT , DTF , XD I FF , YD I FF , RANGE , XS 1 , YS 1 , BRG 1 , BRKKM 1 

REAL*4  DATE , HR , MN , LAT , LONG , TOTIM , TIME ,TIMEM1 , DATE 1 

REAL*4  OBSERR( 300 ) , FAC 1 , SIGTH2 , SIGVT2 , R( 2 , 2) , ETOTAL , EAVG , RTOD 

REAL*4  X2,YS2,BRG2,ZX,ZY,M1,E1,E1M1,E1M2,DTOR,TRKERR(300) 

REAL*4  M2 , E2 , E2M1 ,E2M2 , G1 1 , G13 , G2 1 , G23 , ZXM1 , ZYM1 

REAL*4  XKKS(4, 1,300) ,PKKS(4,4,300) 

RE  AL*4  XNNM 1 ( 4 , 1 ) , X S S ( 4 , 1 ) , XKKM 1 S ( 4 , 1 ) 

REAL*4  PNNM1(4,4) ,PSS(4,4) ,PKKM1S(4,4) ,IPKKM1S(4,4) 

REAL*4  AK( 4 , 4) , AKT( 4, 4), 11(4, 4), STRKERR( 300 ) , DTS( 300 ) 

REAL*4  TEMP1S(4,4) ,TEMP2S(4, 1) ,TEMP3S(4, 1) 

REAL*4  TEMP4S(4,4) ,TEMP5S(4,4) ,TEMP6S(4,4) 

REAL*4  AS,ASA,ASL, NAV , MET 
INTEGER* 2  NP 
INTEGER*,  PCN 
C  OPEN  OUTPUT  DATA  FILES 

0PEN(UNIT=2 ,FILE=' STORM 1.  DAT' ,STATUS=’OLD’ ) 

0PEN(UNIT=3 .FILE  =' OUTDATA. DAT' ,STATUS='NEW' ) 

OPEN( UNIT=4 , FILE= ' TRUDATA. DAT' ,STATUS=’NEW' ) 

OPEN(UNIT=5 ,FILE='FILDATA.  DAT' .STATUS** 'NEW' ) 

OPEN( UNIT=6 , FILE= ' SMDATA.  DAT' ,STATUS='NEW' ) 

OPEN(UNIT=7 ,FILE-' ELLIPDAT.  DAT' .STATUS*' NEW' ) 

OPEN(UNIT=8, FILE*' MATRIX. DAT' . STATUS*' NEW ' ) 

OPEN( UNIT=9 , FILE= ' ERRDATA. DAT f , STATUS= ’ NEW ' ) 

0PEN(UNIT=10, FILE=’ ERRSDATA.DAT' , STATUS*' NEW' ) 

C  RABIAN/DEGREE  CONVERSION  FACTORS 
RTOD=57. 29577951 
DTOR=0.  01745293 

C  COMPUTE  4X4  IDENTITY  MATRIX 
DO  5  1*1,4 
DO  5  J=l,4 
iF  (I.EQ.J)  THEN 

IMAT(I,J)=1. 0 

ELSE 

IMAT( I , J)=0. 0 

END  IF 

5  CONTINUE 

DO  6  1=1,2 
DO  6  J=1 ,4 
H( I , J)=0. 0 

6  CONTINUE 
II(1,1)=1.0 
H(2,3)=l. 0 

C  INITIALIZE  TIME  COUNTER 
T0TTIM=0. 0 
TIMEMi=0. 0 
N?=0 


C  INITIALIZE  COUNTER  FOR  MANEUVER  GATE 
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E1M1=0. 0 
E1M2=0.  0 

COMPUTE  BEARING  MEASUREMENT  COVARIANCE 

BEARING  ERROR  STANDARD  DEVIATION  =  1  NM 
WRITEC*,*)  'FILTERING  OBSERVED  DATA  WITH  KALMAN  FILTER' 
WRITE  ( * ,  * )  '  ***===*** ' 

810  READ(2, 1001,END=800)DATE,HR,MN,LAT,A,L0NG,B,PCN,NAV,MET 
C  SATELLITE  DATA  FOR  MEASUREMENT  NOISE  COV.  MATRIX  VALUES 
IF(PCN. EQ. 1)THEN 
AS=256.  0 

ELSEIF(PCN.  EQ.  3) THEN 
AS=900. 0 

ELSEIF(PCN. EQ. 5)THEN 
AS=1600.  0 
C  RADAR  DATA 

ELSEIF(PCN.  EQ.  2)THEN 
AS=100.  0 

ELSEIF(PCN.  EQ.  4)THEN 
AS=225.  0 

ELSEIF(PCN.  EQ. 6) THEN 
AS=625. 0 
C  AIRCRAFT  DATA 
ELSE 

AS=( (NAV)**2+(MET)**2)**0.  5 
END  IF 
R(1,1)=AS 
R( 1 ,2)=0.  0 
R(2,l)=0.  0 
R(2,2)-AS 


Q  A*5'f**;V5V**VfV;*****Vf<fVr.V**5V' 


’****V«Hf**>V*>V*A**A>V»V>Wf***A****?WfVnW{A*jV 


C  READ  IN  OBSERVATION  FACPCET  ( DATE ,TIME , LAT, LONG) 
C  DT=TIME(K) -TIME(K-l) 


C  READ( 2,1001 ,END=8  00 ) DATE , HR , MN , LAT , A , LONG , B 

1001  F0RMAT(F6. 0,F2. 0,F2. 0,F3. 0,A1 ,F4. 0, A1 , II ,2(F2.  0)) 

NP=NP+1 

MN=MN/60.  0 
LAT=LAT/10 
LONG=LONG/10 
TIME=HR+MN 

C  WRITE  (3,1)  DATE, HR, MN, LAT, A, LONG, B 

1  FORMATC IX, F7. 0,4X,F3. 0 , IX, F6. 4,6X,F4. 1,A1,3X,F5. 1,A1) 

IF  (NP.  EG.  1)  THEN 
DATE 1=D ATE 
TIMEM1=TIME 
END  IF 
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IF  ( DATE.  NE.  DATE1)  THEN 
TIME=TIME+24 
DT=TIME-TIMEM1 
TIME=TIME-24 
ELSE 

DT=TIME-TIMEM1 

ENDIF 

DTF=DT*60.  0 

DTS(NP)=DT 

TOTTIM=TOTTIM+DT 

C  WRITE  (3,2)  TIME,TOTTIM,DT 

2  FORMAT( 1X,F7.  4,5X,F6. 2,SX,F6. 2) 

CALL  FINDPHI(PHI.DT) 

Z(l,l)«lONG 

Z(2,1)=LAT 

ZX=LONG 

ZY=LAT 

IF(NP.EQ.  1)  THEN 

CALL  INIT(LONG,LAT,XKK,PKK) 

C  WRITE(*,*)’X(0|0,0):  ' 

DO  601  1=1,4 
LXKK( I , 1 )=XKK( 1,1) 

C  WRITE(  3  ,*)  '  ************* ’ 

C  WRITE(3,*)  (XKK( I ,1) ,1=1,4) 

601  CONTINUE 


C  WRITE( 3 ,*) 1 P(0 | 0,0):  ' 

DO  602  1=1,4 
DO  602  J=1 ,4 
LPKK( I , J)=FKK( I , J) 
WRITE(3,401)PKK(I,J) 
401  F0RMAT(4F14.4) 

602  CONTINUE 


ENDIF 


C  PROJECT  AHEAD  STATE  AND  ERROR  COVARIANCE  ESTIMATES 
C  X(K+1|K)  =  PHI  *  X C K | K ) 

CALL  MATMULf PHI , XKK , 4 , 4 , 1 , XKKM1 ) 


C 

C 

c 

603 


WRITE(*,*) *X( ' .TIME, ' | ' ,TIMEM1, ' ,0): ’ 
DO  603  1=1,4 

WRITE(3,*)  (XKKM1( I , 1) , 1=1 ,4) 

WRITE( 3 ,*)  ' **************** ' 

LXKKM1( I , 1 )=XKKM1( 1,1) 

CONTINUE 


C  P(K+1 jK)  =  (PHI  *  P(K|K)  *  PHIT)  +  Q 


CALL  MATRAN( PHI , PHIT ,4,4) 

CALL  MATMUL( PHI , PKK, 4 ,4,4 ,TEMP6) 

CALL  MATMULC  TEMP6 , PHIT , 4 , 4 , 4 , TEMP 7 ) 
CALL  GETQ(Q) 

CALL  MATADD(TEMP7,Q,4,4,1,PKKM1) 

DO  408  1=1,4 
DO  408  J=1 ,4 

LPKKMK I ,  J)=PKKM1(  I ,  J) 

408  CONTINUE 

C  VRITEC*,*) ’PC ' ,TIME, ’ | 1 ,TIMEM1, ' ,0):  ' 

DO  604  1=1,4 

C  WRITE(3,402)(PKKM1(I,J),J=1,4) 

402  F0RMAT(4F14.4) 

604  CONTINUE 


204  CONTINUE 


C  COMPUTE  OBSERVATION  RESIDUAL 
C  E=Z( K) -H*X( K | K- 1 ) 

CALL  MATMUL( H , XKKM 1 , 2 , 4 , 1 , TEMP 1 ) 

CALL  MATSUB(Z,T£MP1,2, 1,E) 

C  COMPUTE  VARIANCE  OF  RESIDUALS  SEQUENCE 
C  AND  ADAPTIVE  GATE  VALUE 
C  VAR ( E ) =H* PKKM 1*HT+R 

CALL  MATRAN ( H , HT , 2 , 4 ) 

CALL  MATMULC  H , PKKM 1 , 2 , 4 , 4 ,TEMP2 ) 

CALL  MATMULC TEMP2 ,HT, 2 ,4 , 2 ,TEMP3) 

CALL  MATADDC TEMP3 , R , 2 , 2 , 1 , VARE ) 

C  VRITEC 3,*)’ VARIANCE  OF  RESIDUALS  =  ' ,VARE 

C  GATE1=1. 5*SQRTCVARE) 

C  COMPUTE  KALMAN  GAIN  MATRIX 
C  G=PKKM 1*HT* ( H*PKKM 1*HT+R ) ** - 1 

CALL  MATRAN(H,KT,2,4) 

CALL  MATMULC  PKKM 1 , HT , 4 , 4 , 2 , TEMP4 ) 

CALL  MATINVC VARE , 2 , IVARE ) 

CALL  MATMULC  TEMP4 , 1 VARE , 4 , 2 , 2 , G ) 

C  VRITEC  3 ,*) ' PKKM1*HT  =' 

DO  414  1=1,4 

C  VRITEC 3, *)TEMP4C 1,1) 

414  CONTINUE 

C  VRITEC 3, •'•)  'G  =' 

DO  613  1=1,4 

C  VRITEC  3, *)GC 1 , 1) 

613  CONTINUE 

C  IF  CL.EQ,  1)  THEN 

C  G11=G( 1,1) 

C  G13=GC3,1) 

C  ELSE 
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ENDIF 


G21=G(1,1) 

G23=G(3,1) 


C 

C 

C 


C  COMPUTE  UPDATED  ESTIMATE 

C  X(K|K)=X(K|K-1)+G*E,  WHERE  E=Z(K)-H*X(K|K-1) 
CALL  MATMUL( G ,  E ,  4 , 2 , 1 , TEMP5 ) 

CALL  MATADD(TEMP5 ,XKKM1 , 4,1,1, XKK) 

C  WRITE(3,*)'X( ’ .TIME, ’ j  * .TIME, * , ' ,L, ’ ): ' 

DO  605  1=1,4 

C  WRITE(3,*)XKK(I,1) 

605  CONTINUE 


C  COMPUTE  UPDATED  ERROR  COVARIANCE  MATRIX 
C  P(K|K)=(I  -  G*H)*P(K|K-1) 

CALL  MATMUL( G , H , 4 , 2 , 4 , TEMP6 ) 

CALL  MATSUBC I MAT, TEMP 6 , 4 ,4 , TEMP 7) 

CALL  MATMUL( TEMP  7 , PKKM 1 , 4 , 4 , 4 , PKK ) 

C  WRITE(3, *)'P(’, TIME, T .TIME, ’.’.L,’):  ' 

DO  606  1=1,4 

C  WRITE(3,406)(PKK(I, J) ,J=1,4) 

406  F0RMAT(4F14. 4) 

606  CONTINUE 


C  THESE  STATEMENTS  ARE  FOR  THE  SMOOTHING  ALGORITHM 

DO  620  1=1,4 
XKKS( I , 1,NP)=XKK( I ,1) 

620  CONTINUE 

DO  630  1=1,4 
DO  630  J=1 ,4 

PKKS( I , J,NP)=PKK( I , J) 

630  CONTINUE 


C  COMPUTE  TRUE  TRACKING  ERROR 
ASA=XKK( 1,1) 

ASL=XKK(3, 1) 

TRKERR(NP)=SQRT((LAT-ASA)**2+(L0NG-ASL)**2) 

C  COMPUTE  OBSERVATION  ERROR 

C  OBSERR(NP)=SQRT( (ASLAT-ZX)**2+( ASL0NG-ZY)**2) 

C  SAVE  LATEST  RESIDUALS  FOR  AVERAGING 
C  E1=E 


COMPUTE  THE  AVERAGE  RESIDUAL  OVER  THE  PAST  THREE  OBSERVATIONS 

w /  r*  i  irilii  t  r  i  >  /  r} 

iii.— j 


c 

c 

c 


WRITE( 
VRITEf 
WRITE ( 


) 1  PAST  THREE  RESIDUALS  FOR  SENSOR  1  ARE  : '  ,E1,E1M1,E1M2 
) ' BEARING  AVERAGE  OF  SENSOR  1  =  ' ,M1 
)’ MANEUVER  GATE  FOR  SENSOR  1  =  ' ,GATE1 


o  o  o  o 


E1M2-E1M1 
E1M1=E1 

COMPUTE  ERROR  ELLIPSE  DATA 

CALL  ELLIP(XKK( 1,1) ,XKK(3,1) ,PKK( 1,1) ,PKK(3,3) ,PKK(1,3)) 

C  COMPUTE  ESTIMATED  X-Y  POSITION,  COURSE,  AND  SPEED 
XPOS=XKK( 1, 1) 

YPOS=XKK(3, 1) 

IF  (XKK(2,l).EQ.O  .AND.  XKK(4,1). EQ. 0)  THEN 
HDG=0. 0 

ELSE 

HDG=RTOD* ATAN  2 (XKK(2,1),XKK(4,1)) 

ENDIF 

IF  (HDG.LT.  0.0)  HDG=HDG+360 
SPD=60*SQRT(XKK( 2 , 1)**2+XKK(4 , 1)**2) 

C  WRITE(*,*)  'FILTERED  DATA  FOR  DATA  POINT' ,NP 

WRITE (3,*)  ’FILTERED  DATA  FOR  DATA  POINT' ,NP 

C  WRITE (*,*)  'TIME  X  POS  Y  POS  HEADING  SPEED' 

WRITE (3,*)  'TIME  X  POS  Y  POS  HEADING  SPEED* 

C  WRITE ( * , * ) TOTTIM , XPOS , YPOS , HDG , SPD 

WRITE ( 3 ,*)TOTTIM ,XPOS , YPOS , HDG, SPD 
WRITE( 4 ,*)TOTTIM , ZX , ZY 
WRITE( 5 , * )TOTTIM , XPOS , YPOS , PKK( 1,1) 

WRITE ( 9 , * ) NP , TRKERR( NP ) 

1002  FORMAT( 1X,5F10. 3) 

1003  FORMAT( IX, F6.  2,3X,F10.  1,2X,F11. 1,3X,F8.  1,3X,F8.  1) 

1004  FORMAT( IX, F6.  2,3(F8. 1,2X)) 

C  COMPARE  BEARING  ERRORS  TO  MANEUVER  DETECTION  GATES 

IF  ((ABS(Ml).GT. (GATE1))1  THEN 

WRITE(*, *)'***  MANEUVER  DETECTION  ***' 

C  WRITEC3, *)’***  MANEUVER  DETECTION  ***' 

CALL  REINIT( DT , ZX , ZY , ZXM1 , ZYM1 , LPKKM1 ,XKKM1 , PKKM1 ) 
E1M1=0. 0 
E1M2=0.  0 
GOTO  204 

ENDIF 

TIMEM1=TIME 

DATE1=DATE 


ZXM1=ZX 

ZYM1=ZY 


GOTO  810 


C  THIS  IS  WHERE  THE  SMOOTHING  ALGORITHM  STARTS 
C  FIXED  INTERVAL  SMOOTHING 

800  WRITEC*,*)  'SMOOTHING  FILTERED  DATA  VITO  A' 

WRITE (*,*)  ’FIXED  INTERVAL  SMOOTHING  ALGORITHM' 

i.’d  t  ?r  f  ^  '  *** - ' 
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DO  1000  KK=1,NP-1 
K=NP-KK 


DT=DTS(K+1) 

TIME=TIMEM1-DT 
CALL  FINDPHI(PHI,DT) 

DO  901  1=1,4 
XSS(I,1)=XKKS(I,1,K) 

901  CONTINUE 

DO  902  1=1,4 
DO  902  J=l,4 
PSS( I , J)=PKKS( I , J,K) 

902  CONTINUE 

C  CALCULATE  THE  PREDICTED  STATE  AND  ERROR  COVARIANCE  MATRICES 
C  X(K+1|K)=PHI*X(K|K) 

CALL  MATMUL  (PHI ,XSS, 4,4,1, XKKM1S) 

C  P(K+1 |K)=PHI*P(K|K)*PHIT+Q 

CALL  MATRAN  (PHI ,PHIT,4,4) 

CALL  MATMUL( PHI , PSS , 4 , 4 , 4 , TEMP6 ) 

CALL  MATMUL( TEMP 6 , PHIT , 4 , 4 , 4 , TEMP 7 ) 

CALL  GETQ(Q) 

CALL  MATADD( TEMP7 , Q , 4 , 4 , 1 , PKKM1S ) 


C  CALCULATE  THE  SMOOTHING  FILTER  GAIN  MATRIX 
C  AK=P(K|K)*PHIT*INV°P(K+1|K) 

CALL  MATINV  (PKKM1S,4, IPKKM1S) 

CALL  MATMUL  (PKKM1S,IPKKM1S,4,4,4,II) 
CALL  MATMUL  (PSS ,PHIT, 4,4,4, TEMP1S) 
CALL  MATMUL  (TEMP1S,IPKKM1S,4,4,4,AK) 

DO  904  1=1,4 

XNNM1( I , 1 )=XKKS( I , 1 , K+l ) 

904  CONTINUE 

C  CALCULATE  THE  SMOOTHED  STATE  ESTIMATE 
C  XKKS=X(K|K)+AK*(X(K+1 |N)-X(K+1 |K) 

CALL  MATSUB  ( XNNM 1 , XKKM 1S,4,1,TEMP2S) 
CALL  MATMUL  (AK,TEMP2S,4,4,1,TEMP3S) 
CALL  MATADD  (XSS,TEMP3S,4, 1,K,XKKS) 

DO  906  1=1,4 
DO  906  J=1 ,4 

PNNM J  ( I , J) =PKKS ( I , J , K+ 1 ) 

906  CONTINUE 

C  CALCULATE  THE  SMOOTHED  COVARIANCE  MATRIX 
C  PKKS=P( K | K)+AK*{  P(K+1|N)-P(K+1|K)]*AKT 

CALL  MATSUB  (PNNM1,PKKM1S,4,4,TEMP4S) 
CALL  MATRAN  ( AK , AKT , A , 4 ) 

CALL  MATMUL  ( AK , TEMP4 S , 4 , 4 , 4 , TEMPS  S ) 


CALL  MATMUL  (TEMP5S,AKT, 4,4,4, TEMP6S) 

CALL  MATADD  (PSS,TEMP6S,4,4,K,PKKS) 

C  COMPUTE  ESTIMATED  X-Y  POSITION,  COURSE,  AND  SPEED 
SXPOS=XKKS( 1, 1,K) 

SYPOS=XKKS(3,l,K) 

IF  (XXKS(2,l,K).EQ.O  .AND.  XKKS(4, 1,K). EQ.  0)  THEN 
SHDG=0.  0 

ELSE 

SHDG=RT0D*ATAN2( XKKS ( 2 , 1 , K) , XKKS ( 4 , 1 , K ) ) 

ENDIF 

IF  (SHDG.LT.  0.0)  SHDG=SHDG+360 
SSPD=60*SQRT(XKKS( 2 , 1 ,K)**2+XKKS( 4 , 1 ,K)**2) 

C  WRITE(*,*)  'SMOOTHED  DATA  FOR  DATA  POINT' ,K 

WRITE (3,*)  'SMOOTHED  DATA  FOR  DATA  POINT' ,K 
C  WRITE(*,*)  'TIME  X  POS  Y  POS  HEADING  SPEED' 

WRITE(3,*)  'TIME  X  POS  Y  POS  HEADING  SPEED’ 

C  WRITE( * , *)TOTTIM , SXPOS , S YPOS , SHDG , SSPD 

WRITE( 3 , *)TOTTIM , SXPOS , SYPOS , SHDG , SSPD 
1010  FORMAT( 1X,5F10.  3) 

1020  FORMAT( IX, F6.  2,3X,F10.  1,2X,F11.  1,3X,F8.  1,3X,F8. 1) 

1030  FORMAT( 1X,F6.  2,3(F8.  1,2X)) 

TIMEM1=TIME 
1000  CONTINUE 

C  CL0SE(UNIT=4) 

C  CALCULATE  THE  SMOOTHED  TRACKING  ERROR 
C  OPEN ( UNIT=4 , FILE= ' TRUD ATA.  DAT' , STATUS* 'OLD' ) 

DO  1100  K=1,NP 
SXPOS=XKKS( 1 , 1 ,K) 

SYPOS=XKKS ( 3 , 1 , K) 

C  READf 4,1001) DATE , HR , MN , LAT , A , LONG , B , PCN 

STRKERR(K)=SQRT( ( LAT-SXP0S)**2+( LONG- SYPOS )**2) 

WRITE( 6 , 1120)K, SXPOS , SYPOS ,PKKS( 1 , 1 ,K) 

WRITE( 10,*)K,STRKERR(K) 

1100  CONTINUE 

1110  FORMAT ( 14 , 2F8 .  1 ) 

1120  FORMAT(I4,3(FS.  1,2X1) 

1130  FORMAT(I4,3F8.  1) 

CLOSE (UNIT=2) 

CLOSE(UNIT=3) 

CLOSE (UNIT=4) 

CLOSE(UNIT=5) 

CLOSE(UNIT=6) 

CLOSE(UNIT=7) 

CLOSE(UNIT=8) 

CLOSE(UNIT=S) 

Ct  an 

jjvOw  v,  Uua-iv; 

WRITE(*,*)  'FILTERED  &  SMOOTHED  OUTPUT  DATA  IS  LOCATED  IN  THE 
WRITE (•'■',*)  'DATA  FILE  OUTDATA.DAT.  FOR  GRAPHIC  RESULTS,' 
WRITE (*,*)  'ENSURE  OBSDATA.DAT,  FILDATA.DAT,  &  SMDATA.DAT  ARE 


O  O  O  Cl 


WRITE (*,*)  ’IN  THE  MATLAB  SUB-DIRECTORY  AND  RUN  THE  MATLAB’ 
KRITEC*,*)  'M-FILE  STORM2.M' 

STOP 

END 


C  COMPUTES  THE  VALUES  OF  THE  PHI  MATRIX 

0  Vtrt*iWr*yn'nYrt**yf*jV*5V******yf*iY*yr**5WriWnV5V*rt*ycrtiY*VWtrt*iY**Vr**Vr 


REAL*4  PHI(4,4) ,DT 

DO  1501  1=1,4 
DO  1501  J=1 ,4 
DO  1501  K=l,2 

PHI( I , J)=0. 0 

1501  CONTINUE 

C  COMPUTE  PHI  MATRIX 
DO  1500  1=1,4 
PHI(I,I)=1. 0 
1500  CONTINUE 

PHI( 1,2)=DT 
PHI(3,4)=DT 

RETURN 

END 


SUBROUTINE  INIT( LONG , LAT , XKK , PKK) 

leirltMeStMrltitMtirlrleMtMtleMtitirlrSeicltitMrlrMtMeMtltMtMrlt 

THIS  ROUTINE  INITIALIZES  THE  STATE 
AND  ERROR  COVARIANCE  ESTIMATES 

iritMeMtMririn'tiritrltirMrirltlrliirirttitlfliitic'liirirtt'iiitMritMricir/clt 


REAL*4  XKK(4, 1) ,PKK(4,4) 
REAL”4  LAT, LONG 


C  INITIAL  STATE  ESTIMATE 
XKK(3, 1)=LAT 
XKK(2,1)=0.  0 
XKK( 1 , l)=LONG 
XKK*(4,1)=0.  0 


C  INITIAL  ERROR  COVARIANCE  ESTIMATE 
PKK( 1 , 1)=1G0.  0 
PKK( 1,2)=0.  0 
PKK( 1 , 3)=0.  0 
'  '  PKKC 1 ,4)=0.  0 

PKK( 2 , 1)=0. 0 
PKK(2,2)=0. 025 
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PKK(2,3)=0.  0 
PKK(2,4)=0. 0 
PKK(3,1)=0.  0 
PKK(3,2)=0.  0 
PKK(3,3)=100 
PKK(3,4)=0.  0 
PKK(4, 1)=0.  0 
PKK(4,2)=0.  0 
PKK(4,3)=0.  0 
PKK(4,4)=0.  025 

RETURN 

END 

SUBROUTINE  GETQ(Q) 

C********************************************************* 

C  ROUTINE  TO  GET  Q  MATRIX 

C********************************************************* 

REAL*4  Q(4,4) 

DO  100  1=1,4 
DO  100  J=l,4 
If’"!  Q(  I ,  J)=0.  0 

DO  200  1=1,4 
200  Q(I,I)=100. 

RETURN 

END 


C 

C 

c 

c 


SUBROUTINE  REINIT( DT , ZX , ZY , ZXM1 , ZYM1 , LPKKM1 , XKKM1 , PKKM1 ) 


************* 


■****' 


****** 


************************************* 


THIS  ROUTINE  RE-INITIALIZES  THE  STATE  AND  ERROR 
COVARIANCE  ESTIMATES 


***************** 


**■ 


'******************************* ********* 


RE AL*4  DT , XKKM 1 ( 4 , 1 ) , PKKM 1 ( 4 , 4 ) 
REAL*4  ZX , ZY , ZXM1 , ZYM1 , LPKKM1( 4 , 4 ) 


XDIFF=ZX-ZXM1 

YDIFF=ZY-ZYM1 


XKKM1( 1 , 1)=ZX 
XKKM1( 2 , 1)=XDIFF/DT 
XKKM1(3,1)=ZY 
XKKM1(4, 1)=YDIFF/DT 

C  WRITE( 3,*) 'REINITIALIZED  STATES  ARE:’ 

DO  100  1=1  4 

C  ~  WRITE( 3 ,*)XKKM1 ( 1,1) 

100  CONTINUE 


PKKMK 1 , 1)=2.  25*LPKKM1(  1,1) 

PKKMl(i,2)=0. 0 

PKKM1( 1 , 3) =2.  25*LPKKM1( 1,3) 
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PKKM1( 1,4)=0.  0 
PKKM1(2, 1)=0.  0 
PKKM1(2,2)=0.  1111 
PKKM1(2,3)=0.  0 
PKKM1(2,4)=0.  0 
PKKM1(3, 1)=2. 25*LPKKM1(3, 1) 

PKKM1(3,2)=0.  0 

PKKM1(3 ,3)=2. 25*LPKKM1(3,3) 

PKKM1(3 ,4)=0.  0 
PKKM1(4,1)=0.  0 
PKKM1(4,2)=0.  0 
PKKM1(4,3)=0.  0 
PKKM1(4,4)=0. 1111 

RETURN 

END 

SUBROUTINE  MP( XS 1 , YS 1 , XS2 , YS2 , BRG1 , BRG2 , ZX , ZY) 

THIS  ROUTINE  COMPUTES  THE  ESTIMATED 
X,Y  POSITION  OBTAINED  FROM  MEASUREMENTS 

REALM  ZX.ZY 

REAL*4  XS 1 , YS 1 , XS  2 , YS2 , BRG 1 , BRG2 
REALM  NUMER,DENOM 

INITIAL  STATE  ESTIMATE 

NUMER=( -YS2*TAN( BRG2) )+( YS1*TAN(BRG1) )+XS2-XSl 
DENOM=TAN( BRG1) -TAN( BRG2 ) 

ZY=NUMER/DENOM 
ZX=(ZY-YS1) *TAN ( BRG 1 ) +XS 1 

RETURN 

END 


SUBROUTINE  ELLIP(XT,YT,P1,P3,P13) 

THIS  SUBROUTINE  COMPUTES  ERROR  ELLIPSE  DATA 
FROM  ERROR  COVARIANCE  DATA 

iWc»V*Vf**Vf*;Wf*5'f**.V**!VVrVfVf*Vf*iV*5WrVfVfiV?V5V*VfiWf*VrVfVrVf*VrVrVf***»Wr 

DIMENSIONS  AND  DECLARATIONS 

REALM  XT,YT,XP(21),YP(21),A,B,THE1,SIG2X,SIG2Y 
REALM  SX,SY,PT,CT,ST,P1,P13,P3 


A=2*P13 

E—n  «  n 

-ri-rj 

THE 1=0. 5*ATAN2(A,B) 

A=(Pl+P3)/2 

3=0.  0 

IF  (P13.EQ.  0.0)  GOTO  10 


B=P13/SIN(2.  0*THE1) 

10  SIG2X=ABS(A+B) 

SIG2Y=ABS(A-B) 

SX=SIG2X**0. 5 
SY=SIG2Y**0.  5 
PT=3.  141592654/10 
CT=C0S(THE1) 

ST=SIN(THE1) 

DO  100  IE=1,21 

XP( IE)=SX*COS(PT*IE)*CT-SY*SIN(PT*IE)*ST+XT 
YP(IE)=SX*COS(PT*IE)*ST+SY*SIN(PT*IE)*CT+YT 
WRITE( 7 ,*)XP( IE) , CHAR( 9 ) ,  YP( IE) 

100  CONTINUE 
RETURN 
END 


SUBROUTINE  MATMUL(A,B,L,M,N,C) 

Q  *?V5V*?Wf****Vc,**Vf5V**Vr**5V*VnViV5Wf*Vf**»ViV***Vc*>V****?V**?Vilr5V** 

C  THIS  ROUTINE  MULTIPLIES  TOO  MATRICES  TOGETHER 
C  0  C(L,N)  *  A(L,M)  *  B(M,N) 

Q  A*****-.V-,Wr-.Wf****V.-VrVrV:Vr-,ViV'jWr-,V-.W(-.'fiWn't*VnVVr*5'fi'nfrA*>V*>WrAynV*iV* 

C  DIMENSIONS  AND  DECLARATIONS 

REAL*4  A(L,M) ,B(M,N) ,C(L,N) 

DO  10  1=1, L 
DO  10  J=1 ,N 
C(I,J)=0.0 
10  CONTINUE 

DO  100  1=  1 ,L 

DO  100  J=  1 ,N 

DO  100  K=  1,M 

C(I,J)  =  C(I,J)  +  A( I ,K)*B(K, J) 

100  CONTINUE 

RETURN 


END 


SUBROUTINE  MATRAN(A,B,N,M) 

C  THIS  ROUTINE  TRANSPOSES  A  MATRIX 

C  0  B(M,N)  =  A’(N,M) 

Q  V«V*********rtA***V:****V?Vr*rtrtrt**VnV**iV*iVrt*** 

C  DIMENSIONS  AND  DECLARATIONS 

T>?AT*A  A  f  VT  V\  T>  f  M  M 

iVi-lCiXJ  *T  y  / 


DO  100  1=  1,N 
DO  100  J=  1  ,M 
3(J.I)  =  A( I , J) 
100  CONTINUE 
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RETURN 


END 


SUBROUTINE  MATSCL(Q,A,N,M,C)  . 

C  THIS  ROUTINE  MULTIPLIES  A  MATRIX  WITH  A  SCALAR 
,C  0  C(N,M)  =  Q  *  A(N,M) 

C  DIMENSIONS  AND  DECLARATIONS 

REAL*4  A(N,M) ,  C(N,M),  Q 

DO  100  I  =  1,N 

DO  100  J  =  1,M 

C(I,J)  =  Q*A( I, J) 

100  CONTINUE 

RETURN 

END 


SUBROUTINE  MATSUB(A,B ,N,M,C) 

C  THIS  ROUTINE  SUBTRACTS  TWO  MATRICES 

C  0  C(N,M)  =  A(N,M)  -  B(N,M) 

Q  iHr***********Vc?V****************yc***iV>WfiMf)ffiViWr* 

C  DIMENSIONS  AND  DECLARATIONS 

REAL*4  A(N,M),B(N,M) ,C(N,M) 

DO  100  I  =  1,N 

DO  100  J  =  1,M 

C( I , J)=A( I , J)-B(I , J) 

100  CONTINUE 

RETURN 


END 


SUBROUTINE  MATADD ( A , B , N , M , L , C ) 

C  THIS  ROUTINE  ADDS  WO  MATRICES 

C  0  C(N,M)  =  A(N,M)  +  B(N,M) 

QirMrirltirltititiiitrlsirisisin'tiririrititMrltir'eict'risitirin'titMrlritMtin'fMt 

C  DIMENSIONS  AND  DECLARATIONS 

REAL-4  A(N,M) ,B(N,M) ,C(N,M,L) 

DO  100  I  =  1 ,N 

DO  100  J  =  1 ,M 

C( I , J ,L)=A( I , J)+B( I , J) 

100  CONTINUE 


RETURN 

END 
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SUBROUTINE  MATINV  (A,N,C) 

QlV**********VnV**Vr***-,’nHn' 

THIS  ROUTINE  COMPUTES  THE  INVERSE  OF 
A  MATRIX 

C(N,N)=INV  [ A(N,N)] 


C  DIMENSIONS  AND  DECLARATIONS 

REAL*4  A(N,N),C(N,N),D( 20,20) 
DO  100  I  =  1,N 
DO  100  J  =  1,N 
100  D(I,J)=A(I,J) 


DO  115  1=1, N 
DO  115  J=N+1,2*N 
115  D( I , J)=0.  0 

DO  120  1=1, N 
J=I+N 

120  D(I, J)=l. 0 

DO  240  K=1,N 
M“K+1 

IF  (K.EQ.N)  GOTO  180 
L=K 

DO  140  I=M , N 

140  IF  (ABS(D( I ,K) ).  GT.  ABS(D(L,K)))  L=I 

IF  (L.EQ.K)  GOTO  180 


DO  160  J=K,2*N 
TEMP=D(K, J) 

D(K,J)=D(L,J) 

160  D(L, J)=TEMP 

180  DO  185  J=M,2*N 

185  D(K,J)=D(K,J)/D(K,K) 

IF  (K.EQ.  1)  GOTO  220 

M1=K-1 

DO  200  1=1, Ml 
DO  200  J=M,2*N 

200  D( I , J)=D( I , J) -D( I ,K)*D(K, J) 

IF  (K.EQ.N)  GOTO  260 


220  DO  240  I=M,N 

DO  240  J=M , 2*N 

240  D( I , J)=D( I , J)-D( I ,K)*D(K, J) 

260  DO  265  1=1, N 

DO  265  J=1,N 
K=J+N 

265  C(I,J)=D(I,X) 


RETURN 

END 
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APPENDIX  B 


WIND.FOR 


This  a  listing  of  the  WIND.FOR  micro  computer  program  used  to  generate  the  data 
for  the  storm  wind  speed  tracks  presented  in  this  thesis.  In  order  to  run  this  program, 
the  WIND01.DAT  file  must  be  available. 


C  ***VARIABLE  DEFINITIONS*** 


AK 

AKT 

BRG 

BRKKM1 

DBRG 

DT 

DTOR 

E1,E2 

E1M1.E2M1 

E1M2.E2M2 

FAC1 

G 

GATE1 

H 

HDG 

KT 

I 

IMAT 

J 

K 

LPKK 

LPKKM1 

LXKK 

LXKKM1 

Ml,  M2 

PHI 

PHIT 

PI 

PKK 

PKKS 

PKKM1 

PKKM1S 

IPKKM1S 

PSS 

R 

RANGE 

RTOD 

SFD 

TEMP 


SMOOTHING  FILTER  GAIN  MATRIX 
TRANSPOSE  OF  AK 

MEASURED  TARGET  BEARING  IN  RADIANS 
PREDICTED  TARGET  BEARING  MEASUREMENT  IN  RADIANS 
BRG(K|K-1) 

MEASURED  TARGET  BEARING  IN  DEGREES 

TIME  DELAY  BETWEEN  OBSERVATIONS, T(K)  -  T(K1) 

DEGREE  TO  RADIAN  CONVERSION  FACTOR 
MEASUREMENT  RESIDUAL,  Z(K)  -  H(X(K|K-1)) 

MEASUREMENT  RESIDUAL  AT  PREVIOUS  OBSERVATION 
MEASUREMENT  RESIDUAL  TWO  OBSERVATIONS  PREVIOUS 
RECIPROCAL  OF  VARE 
KALMAN  GAIN  VECTOR 

1.  5*STANDARD  DEVIATION  OF  RESIDUAL  PROCESS,  USED  AS  A 
GATE  IN  MANEUVER  DETECTION 
MEASUREMENT  MATRIX 
ESTIMATED  TARGET  HEADING  IN  DEGREES 
TRANSPOSE  OF  H 
COUNTER 

4X4  IDENTITY  MATRIX  ; 

COUNTER 

ITERATION  INTERVAL 

STATE  COVARIANCE  MATRIX  AFTER  PREVIOUS  OBSERVATIONS 
A  PRIORI  STATE  COVARIANCE  ESTIMATE 
STATE  ESTIMATE  AFTER  PREVIOUS  OBSERVATIONS 
A  PRIORI  STATE  ESTIMATE 

AVERAGE  OF  RESIDUALS  OVER  LAST  THREE  OBSERVATIONS 
DISCRETE-TIME  STATE  TRANSITION  MATRIX 
TRANSPOSE  OF  PHI 
3. 141592654 

ESTIMATION  ERROR  COVARIANCE  MATRIX,  P(K|K) 

SMOOTHED  ERROR  COVARIANCE  MATRIX 

PREDICTED  ESTIMATION  ERROR  COVARIANCE  MATRIX,  P(K|K-1 
PREDICTED  ERROR  COVARIANCE  MATRIX  FOR  SMOOTHING,  P(K+ 
INVERSE  OF  PKKM1S 

ERROR  COVARIANCE  MATRIX  FOR  SMOOTHING,  P(K|K) 
MEASUREMENT  NOISE  COVARIANCE 

DISTANCE  FROM  SENSOR  TO  A  PRIORI  TARGET  POSITION 
RADIAN  TO  DEGREE  CONVERSION  FACTOR 
ESTIMATED  TARGET  SPEED  IN  KNOTS 
TEMPORARY  STORAGE  MATRICES  USED  IN  MATRIX 
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VARE 

XDIFF 

XKK 

XKKS 

XKKM1 

XKKM1S 

XPOS 

XS 

XSS 

XT 

YD  IFF 

YPOS 

YS 

YT 

ZX 

ZY 


OPERATIONS 

VARIANCE  OF  RESIDUALS  PROCESS 

DISTANCE  IN  X  DIRECTION  FROM  SENSOR  TO  A  PRIORI 

TARGET  POSITION 

ESTIMATED  TARGET  STATE  VECTOR,  X(K|K) 

SMOOTHED  TARGET  STATE  VECTOR 
PREDICTED  TARGET  STATE  VECTOR,  X(K|K-1) 

PREDICTED  TARGET  STATE  VECTOR  FOR  SMMOTHING,  X(K+J 
ESTIMATED  TARGET  POSITION  IN  X  DIRECTION 
SENSOR  POSITION  IN  X  DIRECTION 
TARGET  STATE  VECTOR  FOR  SMOOTHING,  X(K|K) 

TRUE  TARGET  POSITION  IN  X  DIRECTION 
DISTANCE  IN  Y  DIRECTION  FROM  SENSOR  TO  A  PRIORI 
TARGET  POSITION 

ESTIMATED  TARGET  POSITION  IN  Y  DIRECTION 
SENSOR  POSITION  IN  Y  DIRECTION 
TRUE  TARGET  POSITION  IN  Y  DIRECTION 
OBSERVED  POSITION  IN  X  DIRECTION 
OBSERVED  POSITION  IN  Y  DIRECTION 


C  VARIABLE  DECLARATIONS 
CHARACTER*!  A,B 


REAL*4  XKK( 2,1), XKKM1 ( 2,1), LPKKM1 (2,2), LXKKM1( 2,1) 

REAL*4  H(2 ,2) ,HT(2 ,2) ,G(2, 1) ,TEMP1(2, 1) ,TEMP2(2,2) ,TEMP3(2, 1) 
REAL*4  TEMP4( 2 , 2 ) , TEMP5 ( 2 , 1 ) , TEMP6 ( 2 , 2 ) , TEMP7 (2,2) 

REAL*4  PKK( 2,2) ,PKKM1( 2 , 2) , Z( 1 , 1) 

REAL*4  LXKK( 2,1) ,LPKK(2,2) ,XS( 10) ,YS( 10) ,DBRG( 10) ,BRG 
RE AL*4  PHI ( 2 , 2 ) , PHIT( 2 , 2 ) , IMAT( 2 , 2 ) , XT , YT 
REAL*4  GATE1 ,E( 2 , 1) ,VARE( 2,2), IVARE( 2 ,2) 

REAL*4  DT , DTF , XD IFF , YDIFF , RANGE , XS 1 , YS 1 , BRG 1 , BRKKM 1 

REAL*4  DATE, HR, MN,LAT, LONG, TOTIM, TIME, TIMEM1 ,DATE1 

REAL*4  OBSERR( 300) ,FAC1 , SIGTH2 ,SIGVT2 ,R( 2 , 2) , ETOTAL , E AVG , RTOD 

REAL*4  X2,YS2,BRG2,ZX,ZY,M1,E1,E1M1,E1M2, DTOR , TRKERR( 300 ) 

REAL*4  M2,E2 ,E2M1 ,E2M2 ,G11 ,G13 ,G21 ,G23, ZXM 1 , ZYM 1 

REAL*4  XKKS(  2,1,300'!,  PKKS(  2,2,300) 

REAL*4  XNNM1(2 ,1) ,XSS( 2,1) ,XKKM1S( 2, 1) 

REAL*4  PNNM1( 2 , 2 ) , PSS( 2,2), PKKM1S( 2,2), IPKKM1S( 2,2) 

REAL*4  AK(2,2) ,AKT(2,2) ,11(2,2), STRKERR( 300 ) , DTS( 300 ) 

REAL*4  TEMP1S( 2,2) ,TEMP2S( 2,1) ,TEMP3S( 2,1) 

REAL*4  TEMP4S ( 2 , 2 ) , TEMP5 S ( 2 , 2 ) , TEMP6S ( 2,2) 

REAL*4  AS , ASA , ASL , WIND , VINDD , NAV , MET 
INTEGER*2  NP,ASIM,K 
INTEGER*,  PCN 
C  OPEN  OUTPUT  DATA  FILES 

0PEN(UNIT=2,FILE=’WIND01.  DAT’ , STATUS= ’ OLD ’ ) 

0PEN(UNIT=3,FILE  =’OUTDATA. DAT* ,STATUS=’ NEW’ ) 

0PEN(UNIT=4 ,FILE=’ OBSDATA.  DAT’ ,STATUS=’ NEW’ ) 

0PEN(UNIT=5 ,FILE=’ FILDATA. DAT’ ,STATUS=’NEW’ ) 

OPEN( UNIT=6 ,FILE=’ SMDATA. DAT’ , STATUS= ’ NEW ’ ) 

0PE.N(UNIT=8,FILE  =' MATRIX,  DAT*  ,STATUS=’ NEW' ) 

0PEN(UNIT=9 ,FILE  =’ PALDATA.  DAT’ ,STATUS=’NEW’ ) 

C  RADIAN/DEGREE  CONVERSION  FACTORS 
P  RTOD=57. 29577951 

DT0R=0.  01745293 


64 


C  COMPUTE  4X4  IDENTITY  MATRIX 
DO  5  1=1,2 
DO  5  J=l,2 
IF  (I.EQ.J)  THEN 

IMAT(I,  J)=l.  0 

ELSE 

IMAT(I,J)=0. 0 

ENDIF 

5  CONTINUE 

H( 1, 1)=1.  0 
H(  1,2)=0.  0 


C  INITIALIZE  TIME  COUNTER 
TOTTIM=0.  0 
TIMEM1=0.  0 
WIND=0.  0 
NP=0 


C  INITIALIZE  COUNTER  FOR  MANEUVER  GATE 
E1M1=0. 0 
E1M2=0.  0 

C  COMPUTE  BEARING  MEASUREMENT  COVARIANCE 
C  BEARING  ERROR  STANDARD  DEVIATION  =  1  NM 

VRITEC*,*)  'FILTERING  OBSERVED  DATA  WITH  KALMAN  FILTER' 

VR ITE  ( * ,  * )  1  ***===*** ' 

810  READ(2,1001,END=800)DATE,HR,MN,LAT,A,LONG,B,PCN,WINDD,NAV,MET 

C  RADAR  DATA  FOR  MEASUREMENT  NOISE  COV.  MATRIX 
IF(PCN.EQ.  1)THEN 
AS=100.  0 

ELSEIFCPCN. EQ.  2)THEN 
AS=225.  0 

ELSEIFCPCN.  EQ.  3) THEN 
AS=625. 0 

ELSEIFCPCN.  EQ. 4)THEN 
AS=900. 0 
C  AIRCRAFT  DATA 
ELSE 

AS=( ( NAV ) **2+( MET ) **2 ) **0 . 5 
ENDIF 
RC1,1)=AS 
R( 1 ,2)=0.  0 
R(2,l)=0.  0 
R(2,2)=AS 


0  ***: 


1  #V  A  < 


■*i'.-*Vr-V****:V*****rt*‘,V*;V**********-.’n'?********* 


C  READ  IN  OBSERVATION  PACKET  (DATE, TIME, LAT, LONG) 
0  DT=TIME(K)  -TIME'’?'-!) 


1001  FORMATCF6.  0,F2. 0,F2.  0,F3.  0,A1,F4.  0,A1,I1,F3.  0,2(F2.  0)) 


MN=MN/60. 0 
LAT=LAT/10 
LONG=LONG/10 
TIME=HR+MN 

1  FORMATC IX, F7.  0,.4X,F3.  0,1X,F6. 4,6X,F4.  1,A1,3X,F5.  1,A1) 
NP=NP+1 

IF  (NP.EQ.  1)  THEN 
DATE 1=D ATE 
TIMEM1=TIME 
END  IF 

IF  (DATE. NE. DATE 1)  THEN 
TIME=TIME+24 
DT=TIME-TIMEM1 
TIME=TIME-24 
ELSE 

DT=TIME-TIMEM1 

ENDIF 

DTF=DT*60.  0 
DTS(NP)=DT 
TOTTIM=TOTTIM+DT 
C  WRITE  (*,*)  DT,NP,WINDD 

2  FORMATC IX, F7.  4,SX,F6. 2 ,5X,F6.  2) 

CALL  FINDPKI ( PHI , DT) 

Z( 1 , 1)=WINDD 
ZY=WINDD 

IF( NP.EQ.  1)  THEN 
CALL  INIT(WINDD,XKK,PKK) 

C  WRITE(*,*)'X(0|0,0):  ' 

DO  601  1=1,2 
LXKK( I , 1 )=XKK( 1,1) 

C  WRITE(3  *)  ' **Vr ***iV****** ' 

C  WRITE(*,*)  (XKK(I , 1) ,1=1,2) 

601  CONTINUE 

WRITEC*  *)  ' ************* 1 
WRITEOVO  ZY 


C  WRITE(3,*)’P(0|0,0):  ’ 

DO  602  1=1,2 
DO  602  J=1 , 2 

C  LPKK( I , J)=PKK( I , J) 

C  WRITE( 3 ,401)PKK( I , J) 

401  FORMATC 2F14. 4) 

602  CONTINUE 

ENDIF 

C  PROJECT  AHEAD  STATE  AND  ERROR  COVARIANCE  ESTIMATES 
C  X(K+1|K)  =  PHI  *  X(K | K) 

CALL  MATMULC PHI , XKK , 2 , 2 , 1 , XKKM 1 ) 


DO  603  1=1,2 
LXKKM1(I, 1)=XKKM1(I, 1) 
603  CONTINUE 


C  P(K+1|K)  =  (PHI  *  P(K|K)  *  PHIT)  +  Q 

CALL  MATRAN( PHI , PHIT, 2 , 2 ) 

CALL  MATMULC PHI , PKK ,2,2,2, TEMP6 ) 

CALL  MATMULC  TEMP6 , PHIT ,2,2,2, TEMP 7) 
CALL  GETQ(Q,DT) 

CALL  MATADDC TEMP7 , Q , 2 , 2 , 1 , PKKM1 ) 

DO  408  1=1,2 
DO  408  J=l,2 

LPKKM1( I , J)=PKKM1( I , J) 

408  CONTINUE 


204  CONTINUE 


C  COMPUTE  OBSERVATION  RESIDUAL 
C  E=Z ( K) -H*X( K | K- 1 ) 

IF(WINDD.  EQ. 0)THEN 
E( 1, 1)=0. 0 
E(2,l)=0.  0 
ELSE 

CALL  MATMUL(H,XKKM1 ,2,2,1 , TEMPI) 

CALL  MATSUB ( Z , TEMP 1 , 2 , 1 , E ) 

END  IF 

C  COMPUTE  VARIANCE  OF  RESIDUALS  SEQUENCE 
C  AND  ADAPTIVE  GATE  VALUE 
C  VAR(E)=H*PKKM1*HT+R 

CALL  MATRAN(H,HT,2,2) 

CALL  MATMULC  H , PKKM1 ,2,2,2, TEMP 2 ) 

CALL  MATMULC TEMP2 , HT, 2 , 2 , 2 , TEMP3 ) 

CALL  MATADD( TEMP3 , R , 2 , 2 , 1 , VARE ) 

C  VRITEC 3,*)' VARIANCE  OF  RESIDUALS  =  ' ,VARE 

C  GATE 1=1. 5*SQRT( VARE ) 

C  COMPUTE  KALMAN  GAIN  MATRIX 
C  G=PKKM 1*HT* ( H*PKKM 1 *HT +R ) ** - 1 

CALL  M ATR AN ( H , HT , 2 , 2 ) 

CALL  MATMULC  PKKM 1 , HT , 2 , 2 , 2 , TEMP4 ) 

CALL  MATINVC VARE , 2 , I VARE ) 

CALL  MATMULC  TEMP4 , 1 VARE , 2 , 2 , 1 , G ) 


u  COMPUTE  UPDATED  ESTIMATE 

C  X(K | K ) =X C K j K-l )+G*E ,  WHERE  E=Z(K)-H*X(K|K-1) 

CALL  MATMULC G , E . 2 , 2 , 1 , TEMP5 ) 

CALL  MATADDC TEMP5 ,XKKM1 ,2,1, 1 ,XKK) 


07 


c 


WRITE(3,*),X(,# TIME, T» TIME,  V,L,’):  ’ 
DO  605  1=1,2 
C  WRITE(3,*)XKK(I,1)  ■ 

605  CONTINUE 


C  COMPUTE  UPDATED  ERROR  COVARIANCE  MATRIX 
C  P(K|K)=(I  -  G*H)*P(K|K-1) 

CALL  MATMUL(G,H,2,2,2,TEMP6) 

CALL  MATSUB( IMAT ,TEMP6 ,2,2 ,TEMP7 ) 

,  CALL  MATMUL(TEMP7 , PKKM1 ,2,2,2, PKK) 


C  THESE  STATEMENTS  ARE  FOR  THE  SMOOTHING  ALGORITHM 

DO  620  1=1,2 
XKKS( I , 1 ,NP)=XKK( 1,1) 

C  WRITE( *,*)XKKS( I , 1 ,NP) , PKKS( I , 1 , NP) 

620  CONTINUE 

DO  630  1=1,2 
DO  630  J=l,2 

PKKS(I,J,NP) =PKK( I , J ) 

630  CONTINUE 

WRITE(3,*)  'FILTERED  DATA  FOR  DATA  POINT' ,NP 
WRITE(3,*)  'TIME  VEL.  ACCELL.  HEADING  SPEED' 
VRITEC  3 , * ) TOTTI M , XKK ( 1,1) ,XKK( 2 , 1) 

WRITE ( 4 , * )TOTTIM , ZY 

WRITE( 5 ,*)TOTTIM ,XKK( 1,1) ,XKK( 2,1) ,PKK( 1,1) 

WRITE( 9 ,*)NP 

1002  FORMAT( 1X,5F10.  3) 

1003  FORMAT( IX, F6. 2,3X,F10.  1,2X,F11.  1,3X,F8.  1,3X,F8.  1) 

1004  FORMAT( IX, F6.  2,3(F8. 1,2X)) 

C  COMPARE  BEARING  ERRORS  TO  MANEUVER  DETECTION  GATES 

IF  ((ABS(Ml).GT.  (GATE1)))  THEN 
C  VRITE( *,*)'***  MANEUVER  DETECTION  ***' 

C  WRITE (3,*) '***  MANEUVER  DETECTION  ***’ 

CALL  REINIT(DT,ZY,ZYM1, LPKKM 1 , XKKM 1 , PKKM 1 ) 
E1M1=0. 0 
E1M2=0.  0 
GOTO  204 

ENDIF 

TIMEM1=TIME 
DATE 1=D ATE 

ZYM1=ZY 
GOTO  810 

WRITE ( 6 , * )TOTTIM , XKK( 1 , 1 ) , XKK( 2 , 1 ) , PKK( 1,1) 

C  THIS  IS  WHERE  THE  SMOOTHING  ALGORITHM  STARTS 
C  FIXED  INTERVAL  SMOOTHING 

800  WRITE(*,*)  'SMOOTHING  FILTERED  DATA  WITH  A' 

WRITE(*,*)  'FIXED  INTERVAL  SMOOTHING  ALGORITHM' 
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WRITE(*,*)  '***'■ 


■A* ' 


C  WRITE  (*,*)  DT,NP,WINDD 

DO  1000  KK=1,NP-1 

C  CALL  REINIT(DT,ZY,ZYM1,LPKKM1,XKKM1,PKKM1) 

K*NP-KK 

DT=DTS(K+1) 

TIME=TIMEM1-DT 
TOTTIH=TOTTIM“DT 
CALL  FINDPHI(PHI,DT) 

DO  901  1=1,2 
XSS(I , 1)=XKKS(I , 1,K) 

901  CONTINUE 

DO  902  1=1,2 
DO  902  J=l,2 
PSS( I , J)=PKKS( I , J,K) 

902  CONTINUE 

C  CALCULATE  THE  PREDICTED  STATE  AND  ERROR  COVARIANCE  MATRICES 
C  X(K+1|K)=PHI*X(K|K) 

CALL  MATMUL  (PHI,XSS,2,2,1,XKKM1S) 

C  P(K+1 |K)=PHI*P(K|K)*PHIT+Q 

CALL  MATRAN  ( PHI , PHIT , 2 , 2 ) 

CALL  MATMULC PHI , PSS , 2 , 2 , 2 ,TEMP6) 

CALL  MATMULC TEMP6 , PHIT , 2 , 2 , 2 ,TEMP7 ) 

CALL  GETQ(Q,DT) 

CALL  MATADD( TEMP 7 , Q , 2 , 2 , 1 , PKKM1S ) 


C  CALCULATE  THE  SMOOTHING  FILTER  GAIN  MATRIX 
C  AK=P(K|K)*PHIT*INV°P(K+1|K) 

CALL  MATINV  (PKKM1S,2,IPKKM1S) 

CALL  MATMUL  ( PKKM 1 S , I PKKM IS, 2, 2, 2, II) 
CALL  MATMUL  (PSS, PHIT, 2, 2, 2, TEMP IS) 
CALL  MATMUL  (TEMP1S,IPKKM1S,2,2,2,AK) 

DO  904  1=1,2 

XNNMK 1 , 1)=XKKS(  I ,  l.K+l) 

904  CONTINUE 

C  CALCULATE  THE  SMOOTHED  STATE  ESTIMATE 
C  XKKS=X( K | K ) +AK*( X( K+ 1 | N ) -X( K+ 1 | K) 

CALL  MATSUB  (XNNM1,XKKM1S,2,1,TEMP2S) 
CALL  MATMUL  (AK,TEMP2S,2,2, 1,TEMP3S) 
CALL  MATADD  (XSS,TEMP3S,2,1,K,XKKS) 

DO  906  1=1,2 
DO  906  J=l,2 

PNNM1 ( I , J )=PKKS( I , J , K+l ) 

906  CONTINUE 

C  CALCULATE  THE  SMOOTHED  COVARIANCE  MATRIX 
C  FKKS=P(K i K)+AK*[  P(K+1 1 N) -P(K+1 1 K)]  *AKT 
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CALL  MATSUB  (PNNM1,PKKM1S,2,2,TEMP4S) 

CALL  MATRAN  (AK,AKT,2,2) 

CALL  MATMUL  CAK,TEMP4S,2,2,2,TEMP5S) 

CALL  MATMUL  CTEMP5S,AKT,2,2,2,TEMP6S) 

CALL  MATADD  (PSS,TEMP6S,2,2 ,K,PKKS) 

WRITEC 3,*)  'SMOOTHED  DATA  FOR  DATA  POINT' ,K 
WRITEC3,*)  'TIME  VEL.  ACCEL.  HEADING  SPEED' 
WRITE(3,*)TOTTIM,XKKS( 1,1,K) ,XKKS(2,1,K) 
WRITE(6,*)T0TTIM>XKKS(1,1>K),XKKS(2,1,K),PKKS(1,1,K) 
1010  FORMATC 1X,5F10. 3) 

1020  FORMATC IX, F6.  2, 3X,F10. 1,2X,F11. 1,3X,F8.  1,3X,F8.  1) 

1030  FORMATC IX, F6.  2, 3(F8. 1,2X)) 

TIMEM1=TIME 
1000  CONTINUE 


1100  CONTINUE 

1110  FORMATC 14, 2F8. 1) 

1120  FORMATC 14, 3CF8. 1,2X)) 


CL0SE(UNIT=2) 
CLOSE(UNIT=3) 
CLOSE ( UN IT=4) 
CLOSE(UNIT=5) 
CLOSE(UNIT=6) 
CLOSECUNIT=9) 
CLOSECUNIT=8) 


WRITEC*,*) 

WRITEC*,*) 

WRITEC*,*) 

WRITEC*,*) 

WRITEC*,*) 

STOP 

END 


'FILTERED  &  SMOOTHED  OUTPUT  DATA  IS  LOCATED  IN  THE* 
'DATA  FILE  OUTDATA.DAT.  FOR  GRAPHIC  RESULTS,’ 
'ENSURE  OBSDATA.DAT,  FILDATA.DAT,  &  SMDATA.DAT  ARE' 
'IN  THE  MATLAB  SUB -DIRECTORY  AND  RUN  THE  MATLAB’ 
’M-FILE  STORM2.M’ 


QVriV-.V'!V‘.V-.V-.V-.'r.',",'nVVf.'f.V*-,V-.W.--.ViV;W?5WnViWfVrVriV*)Wr*iV-,WrVo’nV*i'n'nVi'f*A*****iWf***iWr*'iWc*-.V 

C  SUBROUTINES 

Q:WnWr-'nV)VV,-V.-iWrVf;\“.Wf.WfVfVf.WfVn'nV‘,WfVrVr;Wn'?iWf,>-,W,-VnVVnWr5V>WnViWfVrYWriViWnHci'nWoV?':iWnWr 


SUBROUTINE  FINDPHIC PHI ,DT) 

Q  Vo’nV*-.Wf-.'r‘.'f;VVr-.V*;VV;-.VV:**-.V:'r';V*-.V‘.V‘.V-.'nWnV-,V'>WfVnWnVAVf,V*VnV)V>’r****iV*5'nWnV 

C  COMPUTES  THE  VALUES  OF  THE  PHI  MATRIX 

Q  VrVr*;V*‘.V',V-,Mf5V‘.’f.V".Wr.VV.".V'.V*VnWrVnV'<Wf.WnWf)V‘.V,.V-.Wf ■.'?*-V5Wc;V*-.VifrVnWnV‘/V,>V*VnWr 

REAL*4  PHI (2, 2) ,DT 


C  DO  1501  1=3,4 

C  DO  1501  J=1 ,4 

PHICI,J)=0.  0 

C501  CONTINUE 


C  COMPUTE  PHI  MATRIX 
DO  1500  1=1,2 
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PHI(I,I)=1.  0 
1500  CONTINUE 

PHI(1,2)=DT 
PHI(2, 1)=0.  0 
PHI(2,3)=0.0 
PHI(2,4)=0.  0 
PHI( 1,3)=0.  0 
PHI(1,4)=0.  0 

RETURN 

END 


SUBROUTINE  INIT(WINDD,XKK,PKK) 

******iWf>V***?'fiV**?V*****iWf*iWr***iWr***iViHr*)Mf**A 

THIS  ROUTINE  INITIALIZES  THE  STATE 
AND  ERROR  COVARIANCE  ESTIMATES 

VnV**Vf***VfA*******iWf*****ioV****Vf**Vf*5Wfifr****** 

REAL*4  XKK( 1, 1) ,PKK(2,2) 

REAL*4  VIND,WINDD 

INITIAL  STATE  ESTIMATE 
XKK( 1, 1)=WINDD 
WRITE(*,*)  XKK( 1, 1) 

XKK(3,1)=0.  0 
XKK(4, 1)=0.  0 

INITIAL  ERROR  COVARIANCE  ESTIMATE 
PKK( 1 , 1 )=1000000. 

PKK( 1,2)=0.  0 
PKK( 1,3)=0.  0 
PKK(1,4)=0.  0 
PKK( 2 , 1)=0. 0 
PKK( 2 , 2)=0. 25 
PKK(2,3)=0. 0 
PKK(2 ,4)=0. 0 
PKK( 3 , 1)=0. 0 
PKK(3,2)=0. 0 
PKK( 3 , 3)=0. 0 
PKK(3,4)=0. 0 
PKK(4,1)=0.0 
PKK(4,2)=0. 0 
PKK(4,3)=0. 0 
PKK(4,4)=0. 0 

RETURN 


END 


SUBROUTINE  GETQ(Q,DT) 

QiV7ViWfiWf**5'r5V5V**VfiWf*Vf*?VVf*5VVf**;VjVVryrVfVriWr5ViVVfVirififVr*iV**?VyrVf5WfVf**?WoWr* 

C  ROUTINE  TO  GET  Q  MATRIX 

REAL*4  Q(2,2) ,DT 


C  DO  100  1=1,4 

C  DO  100  J=3,4 

COO  Q(I,J)=0. 0 

Q(l,l)=(DT**4)/4. 
Q(  l,2)=(DT**3)/2. 
QC  2 , 1 )=( DT**3 ) / 2. 
Q(2,2)=(DT**2) 

C  DO  200  1=3,4 

C  DO  200  J=l,4 

COO  Q(I,J)=0. 0 

RETURN 

END 


SUBROUTINE  REINIT( DT , ZY , ZYM1 , LPKKM 1 , XKKM1 , PKKM1 ) 

C  THIS  ROUTINE  RE-INITIALIZES  THE  STATE  AND  ERROR 

C  COVARIANCE  ESTIMATES  _______  aaa \aaaaaaaaaaaaaaa 

REAL*4  DT,XKKM1(2,1) ,PKKM1(2,2) 

REAL*4  ZX , Z Y , ZXM1 , ZYM1 , LPKKM 1(2,2) 

C  XDIFF=ZX-ZXM1 

C  YDIFF=ZY-ZYM1 

C  XKKM1(1,1)=ZX 

XKKM1(1,1)=ZY 
C  XKKM1( 3 , 1)=0.  0 

C  XKKM1(4, 1)=0.  0 

WRITE(*,*)’ REINITIALIZED  STATES  ARE:  ' 

DO  100  1=1,2 

VRITE(*,*)XKKM1(I,1) 

100  CONTINUE 

PKKMK 1 , 1)=2.  25*LPKKM1(  1,1) 

PKKM1( 1 ,2)=0. 0 

C  PKKM 1 ( 1 , 3 ) =2 . 25*LPKKM 1(1,3) 

C  PKKM1( 1,4)=0.  0 

PKKM1( 2 , 1)=0.  0 
PKKM1( 2 , 2)=0.  1111 
C  PKKM1(2,3)=0.  0 

C  PKKMK  2 ,4)=0.  0 

C  PKKM1(3,1)=2. 25*LPKKM1(3,1) 

C  PKKM1(3,2)=0. 0 

C  PKKM1(3,3)=2. 25*LPKKM1(3,3) 

C  PKKM1(3,4)=0. 0 

C  PKKM1(4,1)=0. 0 

C  PKKM1(4,2)=0.  0 

C  PKKM1(4,3)=0.  0 

C  PKKM1(4,4)=0.  1111 


RETURN 


nnnoo  o  oooo 


END 


SUBROUTINE  MP( XS 1 , YS 1 , XS2 , YS2 , BRG1 , BRG2 , ZX , ZY ) 


THIS  ROUTINE  COMPUTES  THE  ESTIMATED 
X,Y  POSITION  OBTAINED  FROM  MEASUREMENTS 

REAL*4  ZX,ZY 

REAL*4  XS1 , YS1 ,XS2,YS2,BRG1,BRG2 
REAL*4  NUMER.DENOM 


INITIAL  STATE  ESTIMATE 


NUMER=( -YS2*TAN(BRG2))+(YS1*TAN(BRG1))+XS2-XS1 
DEN0M=TAN( BRG1) -TAN( BRG2) 

ZY=NUMER/DENOM 

ZX=( ZY-YS1)*TAN( BRG1 )+XS 1 

RETURN 


END 


SUBROUTINE  ELLIP(XT,YT,P1,P3,P13) 


THIS  SUBROUTINE  COMPUTES  ERROR  ELLIPSE  DATA 
FROM  ERROR  COVARIANCE  DATA 


***********Vf**iWf**?V**<,nV*i'nWf?V**5'f*******5V***********;V 


DIMENSIONS  AND  DECLARATIONS 

REAL*4  XT,YT,XP(21),YP(21),A,B,THE1,SIG2X,SIG2Y 
REAL*4  SX,SY,PT,CT,ST,P1,P13,P3 


A=2*P13 

B=P1-P3 

THE 1=0. 5*ATAN2(A,B) 

A=(Pl+P3)/2 

B=0.  0 

IF  (P13. EO. 0. 0)  GOTO  10 
B=P13/SIN(2.  0*THE 1 ) 

10  SIG2X=ABS(A+B) 
SIG2Y=ABS(A-B) 
SX=SIG2X**0. 5 
SY=SIG2Y**0.  5 
PT=3.  141592654/10 
CT=C0S(THE1) 
ST=SIN(THE1) 


DO  100  IE=1 ,21 

XP(  IE)=SX*COS(PT*IE)*CT-SY*SIN(  PT*IE)*ST+XT 
YP( IE )=SX*COS( PT* IE )*ST+SY*SIN( PT*IE)*CT+YT 
WRITE( 7 ,*)XP( IE) ,CHAR(9) ,YP( IE) 

100  CONTINUE 


RETURN 


END 


C 

C 

c 

c 

c 


SUBROUTINE  MATMUL(A)B,L,M,N,C) 


THIS  ROUTINE  MULTIPLIES  TWO  MATRICES  TOGETHER 
0  C(L,N)  =  A(L,M)  *  B(M,N) 


DIMENSIONS  AND  DECLARATIONS 
REAL*4  A(L,M) ,B(M,N) ,C(L,N) 


DO  10  1=1 ,L 
DO  10  J=1,N 
C(I,J)=0.  0 
10  CONTINUE 


DO  100  1=  1 ,L 
DO  100  J=  1 ,N 
DO  100  K=  1,M 

C(I,J)  =  CCIjJ)  +  A(I,K)*B(K,J) 
100  CONTINUE 


RETURN 

END 


SUBROUTINE  MATRAN(A,B,N,M) 

Q  ynVVc*iVVr*****?WfiV****Vf****5VVfiViW;Vf*>'f>ViWc*iV?V*AA 

C  THIS  ROUTINE  TRANSPOSES  A  MATRIX 

C  0  B(M,N)  =  A'(N,M) 

Q  ********V:***iV***5'f*5f :>*************!>****** 

C  DIMENSIONS  AND  DECLARATIONS 

REAL*4  A(N,M),  B(M,N) 

DO  100  1=  1,N 
DO  100  J=  1 ,M 
B( J, I)  =  A(I,J) 

100  CONTINUE 

RETURN 

END 


C 

C 

C 


SUBROUTINE  MATSCL(Q,A,N,M,C) 


>Wo'nV>Wc;';  ***  *  ***  * 


******* 


*  *  *  *  *  *  *  *  *  **  **  *  *  *  *  *  * 


THIS  ROUTINE  MULTIPLIES  A  MATRIX  WITH  A  SCALAR 
0  C(N,M)  =  Q  *  A(N,M) 


DIMENSIONS  AND  DECLARATIONS 

REAL*4  A(N,M),  C(N,M),  Q 


DO  100  I  =  1  ,N 
DO  100  J  =  1,M 
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C( I , J)  *  Q*A(I,J) 
100  CONTINUE 

RETURN 

END 


C 

n 

c 

c 

c 


SUBROUTINE  MATSUB(A,B,N,M,C) 


THIS  ROUTINE  SUBTRACTS  TWO  MATRICES 
•  C(  N^M)  =^A(  N ,  MV-  ^B(  N^i) 

DIMENSIONS  AND  DECLARATIONS 
REAL*4  A(N,M) ,B(N,M) ,C(N,M) 


DO  100  I  =  1,N 
DO  100  J  =  1 ,M 
C(I,J)=A(I,J)-B(I,J) 
100  CONTINUE 


RETURN 


END 


SUBROUTINE  MATADD(A,B,N,M,L,C) 

C*****iWfVf?Y5V****Vf****?’r5':******5YA*yc**AiWf*Vf***Vf*** 

C  THIS  ROUTINE  ADDS  TWO  MATRICES 

C  0  C(N,M)  =  A(N,M)  +  B(N,M) 

C  DIMENSIONS  AND  DECLARATIONS 

REAL*4  A(N,M) ,B(N,M) ,C(N,M,L) 

DO  100  I  =  1,N 

DO  100  J  =  1,M 

C(I,J.L)=A(I,J)+B(I,J) 

100  CONTINUE 


RETURN- 

END 


(JlVlYiViViV: 

c 

c 

c 

c 


100 


SUBROUTINE  MATINV  (A,N,C) 


■ir 


rklettirlr. 


!)V*t'nWnV*!Vt'()ViV)V)W(Wn't* 


THIS  ROUTINE  COMPUTES  THE  INVERSE  OF 
A  MATRIX 

C(N,N)=INV  [ A(N,N)] 


DIMENSIONS  AND  DECLARATIONS 
REAL*4  A( N , N) , 0 ( N , N) , D( 20 , 20 ) 
DO  100  I  =  1,N 
DO  100  J  =  1,N 
DC  I , J1=Ar T  TY 
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DO  115  1=1, N 
DO  115  J=Nrl ,2*N 
D( I , J  J=0. 0 


120 


DO  120  1=1, N 
J=I+N 
D(I, J)=l.  0 

DO  240  K=1,N 
M=K+1 

IF  (K.EQ.N)  GOTO  180 
L=K 

DO  140  I=M,N 
140  IF  (ABS(D(I,K)).GT.ABS(D(L,K)))  L=I 

IF  (L.EQ.K)  GOTO  180 

DO  160  J=K,2*N 
TEMP=D(K, J) 

D(K, J)=D(L, J) 

160  D(L, J)=TEMP 

180  DO  185  J=M , 2*N 

185  D(K,J)=D(K, J)/D(K,K) 

IF  (K.EQ. 1)  GOTO  220 
M1=K-1 

DO  200  1=1, Ml 
DO  200  J=M , 2*N 

200  D(I,J)=D(I,J)-D(I,K)*D(K,J) 

IF  (K.EQ.N)  GOTO  260 

220  DO  240  I=M,N 

DO  240  J=M,2*N 

240  D(I,J)=D(I,J)-D(I,K)*D(K,J) 

260  DO  265  1=1, N 

DO  265  J=1,N 
K=J+N 

265  C(I,J)=D(I,K) 

RETURN 

END 
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Monterey,  CA  93943-5000 

6.  Gcncl  Kunnay  Baskanligi 
Personcl  Dairesi 

Bakanlikiar  -  Ankara  /  TURKEY 

7.  AR-OE 

Bakanlikiar  -  Ankara  I  1 URKEY 

8.  Deniz  Kuvvctlcri  Komutanligi 
Personcl  Dairc  Baskanligi 
Bakanliklar-Ankara  !  ’1  URKEY 

9.  Kara  Harp  Okulu  Komutanligi 
Okul  Kutuphanesi  ve 
Elektrik  Bolumu  Kutuphanesi 
Bakanlikiar  -  Ankara  /  TURKEY 

10.  Deniz  Harp  Okulu  Komutanligi 
Okul  Kutuphanesi  ve 
Elektrik  Bolumu  Kutuphanesi 

1  uzia  -  Istanbul /  '1  URKEY 


11.  I  lava  I  larp  Okuiu  Komutanligi 
Okul  Kutuphancsi  vc 
Elektrik  Bolumu  Kutuphancsi 
Istanbul TURKEY 

12.  Fakultc  vc  Yuksck  Okullar  Komutanligi 
Kutuphancsi 

Dikimcvi  -  Ankara  /  TURKEY 

13.  Fakulte  ve  Yuksck  Okullar  Komutanligi 
Kutuphanesi 

Cankurtaran  -  Istanbul /  TURKEY 

14.  Ciurkan  TURKES 
NI’GS.  SMC#  2983 
Monterey,  CA  93943 

15.  Asim  MUTAF 

Escndcre  Mali.  52/29  sk.  No.:14/2 
Guzclvali- Izmir  /  TURKEY 

16.  Bogazici  Univcrsitesi 
Elektrik  Fakultcsi 
Istanbul  I  TURKEY' 

17.  Ortadogu  Univcrsitesi 
Elektrik  Fakultcsi 
Ankara  /  TURKEY’ 

IS.  Istanbul  Tcknik  Univcrsitesi 
Elektrik  Fakultcsi 
Istanbul  /  TURKEY' 

19.  Cinarli  Tcknik  Liscsi 
Elektrik  Bolumu 
Cinarli-Izmir;  IURKE.Y 


7«> 


